Attitude control device of mobile robot

ABSTRACT

A posture control system for a mobile robot. When an unexpected external force acts, the system is configured to control and stabilize the posture of the robot driving an arm link such that, in response to a first external force that is a component in a predetermined direction of an unexpected external force, a second external force acts on the arm link in a direction orthogonal to the predetermined direction. With this, when the mobile robot receives a reaction force, even if the posture becomes unstable or the robot receives an unexpected reaction force, it becomes possible to preserve the dynamic balance and to maintain a stable posture.

TECHNICAL FIELD TO WHICH THE INVENTION RELATES TO

The present invention relates to a posture control system of a mobilerobot, and specifically to a control system for ensuring a stableposture of a mobile robot, in particular a biped mobile robot or a wheelmobile robot having an arm(s), when the robot is controlled to perform awork that is likely to receive a reaction force from an object throughthe arm.

BACKGROUND ART OF THE INVENTION

The applicant discloses a posture control system of a legged mobilerobot, for mobile robots, in Japanese Laid-Open Patent Application No.Hei 10 (1998)-230485. In this prior art, it is configured to displacethe position of the center of gravity of the robot by correcting aposture gradually over a long period of time to displace a body of therobot so as to generate a moment of gravity for compensation, so as tomaintain or preserve the dynamic balance, in particular the equilibriumthat balances robot posture dynamically, when the robot receives anunexpected reaction force from an object. At the same time it isconfigured, during a transient period, to preserve the dynamic balanceby driving a foot to move to take a posture to brace legs, so as togenerate a total floor reaction force's moment for compensation about adesired floor reaction force central point (that is equal to a desiredZMP).

Thus, in the aforesaid prior art, if unexpected reaction force actingfrom the object changes abruptly, at that moment (in a short period oftime), the robot is operated to move the foot to brace legs in order tomaintain or preserve the dynamic balance. That is to say, the prior artensures to stabilize the posture by generating the total floor reactionforce's moment.

Further, the applicant proposes a technique (in Japanese Laid-OpenPatent Application No. Hei 10 (1998)-277969) to generate a moment as atotal floor reaction force's moment actively to restore a posture of therobot when the posture is tilted or inclined, regardless of whether therobot receives a reaction force from the object. However, since thetotal floor reaction force's moment generated by the aforesaid techniquehas a limit, if trying to generate the total floor reaction force'smoment beyond the limit, a part of a foot may be lifted up, resulting inthe robot losing the dynamic balance or, at worst, tipping over.

Furthermore, the applicant also proposes a technique to maintain thedynamic balance by correcting a body trajectory of a robot, instead ofgenerating a total floor reaction force's moment (in Japanese Patent No.3269852). In the prior art, however, since a foot landing position,etc., is needed to be corrected, if the robot is controlled to work withthe object by using the arm, the body or foot may sometimes be likelymove from an appropriate position for working.

In addition, in the case that the robot is controlled to work to impartsome motion to an object, in order to accomplish a task as expected, itis preferable not to influence on a motion of the object, when a controlto maintain the dynamic balance or to restore a tilted or inclinedposture of the robot is implemented.

DISCLOSURE OF THE INVENTION

A first object of the present invention is to eliminate theabove-mentioned drawbacks, and to provide a posture control system of amobile robot that can preserve the dynamic balance so as to maintain astable posture, if the robot loses a stable posture or if the robotreceives an unexpected reaction force from an object, when the robotperforms a work that is likely to receive a reaction force from theobject.

Further, a second object of the present invention is to provide aposture control system of a mobile robot that can preserve the dynamicbalance so as to maintain a stable posture, if the robot loses a stableposture or if the robot receives an unexpected reaction force from anobject, when the robot performs a work that is likely to receive areaction force from the object, without operating or manipulating afloor reaction force acting from the surface which the robot contacts orwith decreasing the amount of the manipulated variable of the floorreaction force.

Further, a third object of the present invention is to provide a posturecontrol system of a mobile robot that can preserve the dynamic balanceso as to maintain a stable posture, if the robot loses a stable postureor if the robot is subject to an unexpected reaction force acting froman object, when the robot is controlled to perform a work that is likelyto receive a reaction force from the object, without giving an influenceon a motion of the object.

In order to achieve the first object, as recited in claim 1 mentionedbelow, the present invention is arranged to provide a posture controlsystem of a mobile robot having at least a body, a locomotive mechanismand at least one arm link that can act a force on an object;characterized in that; when unexpected external force acts, the arm linkis driven such that, in response to a first external force that is acomponent in a predetermined direction of the unexpected external force,a second external force acts on the arm link in a direction orthogonalto the predetermined direction, whereby a posture of the robot isstabilized. Thus, in the posture control system of a mobile robot, whenunexpected external force acts, it is configured to control to stabilizethe posture of the robot driving the arm link such that, in response tothe first external force that is the component in a predetermineddirection of the unexpected external force, the second external forceacts on the arm link in a direction orthogonal to the predetermineddirection. With this, when the mobile robot is controlled to work thatis likely to receive a reaction force from an object, even if theposture becomes unstable or the robot receives the unexpected reactionforce acting from the object, it becomes possible to preserve thedynamic balance and to maintain a stable posture.

As recited in claim 2 mentioned below, the present invention is arrangedto provide a posture control system of a mobile robot having at least abody, a locomotive mechanism and at least one arm link that can act aforce on an object; characterized in that: desired motion generatingmeans for generating desired motion comprising at least a desired armlink position/posture of the robot and a desired object reaction forcethat is a desired value of an object reaction force acting on the armlink from the object; actual object reaction force detecting means fordetecting or estimating an actual object reaction force that is anactual value of the object reaction force; posture stabilizationcontrolling means for correcting the desired motion to stabilize aposture of the robot based on at least the detected or estimated actualobject reaction force; and an arm driver driving the arm link based onat least the corrected desired motion. Thus, the posture control systemof a mobile robot is configured to generate the desired motioncomprising at least the desired arm link position/posture of the robotand the desired object reaction force that is a desired value of theobject reaction force acting on the arm link from the object; to detector estimate the actual object reaction force that is the actual value ofthe object reaction force; to correct the desired motion to stabilizethe posture of the robot based on at least the detected or estimatedactual object reaction force; and to include the arm driver that drivesthe arm link based on at least the corrected desired motion. With this,when the mobile robot is controlled to work that is likely to receive areaction force from an object, even if the posture becomes unstable orthe robot receives the unexpected reaction force acting from the object,it becomes possible to preserve the dynamic balance and to maintain astable posture.

As recited in claim 3 mentioned below, the present invention is arrangedsuch that the posture stabilization controlling means corrects thedesired motion to stabilize the posture of the robot based on at leastan object reaction force's error indicating a difference between theactual object reaction force and the desired object reaction force.Thus, since the desired motion is corrected to stabilize the posture ofthe robot based on at least the object reaction force's error indicatingthe difference between the actual object reaction force and the desiredobject reaction force, it becomes possible to obtain the aforesaideffects and advantages to a further extent.

As recited in claim 4 mentioned below, the present invention is arrangedsuch that the posture stabilization controlling means corrects thedesired motion to cancel out all or a part of tilting force generated bya first component of the object reaction force's error by a secondcomponent obtained from the first component. Thus, since the desiredmotion is corrected to cancel out all or a part of tilting forcegenerated by the first component of the object reaction force's error bythe second component obtained from the first component, it becomespossible to obtain the aforesaid effects and advantages to a furtherextent.

As recited in claim 5 mentioned below, the present invention is arrangedto provide a posture control system of a mobile robot having at least abody, a locomotive mechanism and at least one arm link that can act aforce on an object; characterized in that: desired motion generatingmeans for generating desired motion comprising at least a desired armlink position/posture of the robot and a desired object reaction forcethat is a desired value of an object reaction force acting on the armlink from the object; posture stabilization controlling means fordetecting a posture inclination error of the robot and for correctingthe desired motion such that the posture inclination error approacheszero; and an arm driver driving the arm link based on at least thecorrected desired motion. Thus the posture control system of a mobilerobot is configured to generate the desired motion comprising at leastthe desired arm link position/posture of the robot and the desiredobject reaction force that is the desired value of the object reactionforce acting on the arm link from the object; to detect the postureinclination error of the robot and to correct the desired motion suchthat the posture inclination error approaches zero; and to drive the armlink based on at least the corrected desired motion. With this, when themobile robot is controlled to work that is likely to receive a reactionforce from an object, even if the posture becomes unstable or the robotreceives the unexpected reaction force acting from the object, itbecomes possible to preserve the dynamic balance and to maintain astable posture.

It should be noted that, in claims and detailed description of thepreferred embodiments, the “mobile robot” includes a mobile robot thatreceives the object reaction force through a portion(s) other than thearm link(s). With respect to the arm links, it should also be noted thatthe arm links would include any link(s), even the leg link(s) if the leglink(s) can act on the object. For example, if the robot is in aninsect-type having six leg links which lifts the object using the twoforeleg links, the two foreleg links should be regarded as the armlink(s).

It should further be noted that the “position/posture” is used toindicate the “position and/or posture”. In other words, the“position/posture” does not necessarily include both “position” and“posture”.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a front view of a legged mobile robot to which a posturecontrol system of a mobile robot according to an embodiment of thepresent invention is applied;

FIG. 2 is a side view of the robot illustrated in FIG. 1;

FIG. 3 is an explanatory view of the robot illustrated in FIG. 1 in askeleton manner;

FIG. 4 is a block diagram showing in detail an electronic control unit(ECU) or the like illustrated in FIG. 3;

FIG. 5 is a block diagram showing the configuration of the posturecontrol system of a mobile robot according to the embodiment of thepresent invention;

FIG. 6 is an explanatory view showing the supporting leg coordinatesystem in a gait generated by a desired working pattern generator of thesystem illustrated in FIG. 5;

FIG. 7 is an explanatory view, similar to FIG. 6, and showing thesupporting leg coordinate system in a gait generated by the desiredworking pattern generator of the system illustrated in FIG. 3;

FIG. 8 is a block diagram showing the configuration of a compositecompliance controller applied to a leg main controller.

FIG. 9 is an explanatory view showing a principle on which the systemillustrated in FIG. 5 is premised;

FIG. 10 is an explanatory view, similar to FIG. 9, and also showing theprinciple on which the system illustrated in FIG. 5 is premised;

FIG. 11 is an explanatory view, similar to FIG. 9, and also showing theprinciple on which the system illustrated in FIG. 5 is premised;

FIG. 12 is an explanatory view, similar to FIG. 9, and also showing theprinciple on which the system illustrated in FIG. 5 is premised;

FIG. 13 is an explanatory view, similar to FIG. 9, and also showing theprinciple on which the system illustrated in FIG. 5 is premised;

FIG. 14 is a block diagram showing the configuration of a posturestabilization main controller among the systems illustrated in FIG. 5;

FIG. 15 is a block diagram showing the configuration of a posturestabilization moment calculator of the posture stabilization maincontroller illustrated in FIG. 14;

FIG. 16 is a block diagram showing the configuration of an objectreaction force moment error separator of the posture stabilization maincontroller illustrated in FIG. 14;

FIG. 17 is a block diagram showing the configuration of an objectreaction force equilibrium control distributor of the posturestabilization main controller illustrated in FIG. 14;

FIG. 18 is a block diagram, similar to FIG. 17, and similarly showingthe configuration of the object reaction force equilibrium controldistributor of the posture stabilization main controller illustrated inFIG. 14;

FIG. 19 is a block diagram showing the configuration of an objectreaction force equilibrium controller of the posture stabilization maincontroller illustrated in FIG. 14;

FIG. 20 is a block diagram showing the configuration of a compensatingmoment distributor of the posture stabilization main controllerillustrated in FIG. 14;

FIG. 21 is a block diagram showing the configuration of a correcteddesired object reaction force calculator of the posture stabilizationmain controller illustrated in FIG. 14;

FIG. 22 is a block diagram showing a modified and simplified controlsystem obtained with focus on the object reaction force, in order toexplain the effects and advantages of the system illustrated in FIG. 5;

FIG. 23 is a block diagram also showing a modified and simplifiedcontrol system obtained with focus on the object reaction force, inorder to explain the effects and advantages of the system illustrated inFIG. 5;

FIG. 24 is an explanatory view showing the operation of the posturestabilization main controller illustrated in FIG. 14;

FIG. 25 is a block diagram, similar to FIG. 14, but showing theconfiguration of a posture stabilization main controller in a posturecontrol system of a mobile robot according to a second embodiment of thepresent invention; and

FIG. 26 is an explanatory view showing the configuration of a posturecontrol system of a mobile robot according to a third embodiment of thepresent invention.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

A posture control system of a mobile robot according to an embodimentwill be explained with reference to the accompanied drawings.

FIG. 1 is a front view of a legged mobile robot to which the posturecontrol system of the mobile robot according to the embodiment isapplied and FIG. 2 is a side view of the robot. Here, the legged mobilerobot in a humanoid type that has two leg links and two arm links istaken as an example of the mobile robot.

As illustrated in FIG. 1, the legged mobile robot (hereinafter referredto as “robot”) 1 has a plurality of, specifically two leg links (orlegs) 2 and a body (trunk) 3 on the upper side thereof. A head 4 isprovided above the body 3 and two arm links (arms) 5 are each connectedto the both sides of the body 3. A housing section 6 is disposed on theback of the body 3 and an electronic control unit (explained later) orthe like is installed therein. The robot 1 illustrated in FIGS. 1 and 2is covered to protect the internal structure from being damaged.

FIG. 3 is an explanatory view of the robot illustrated in FIG. 1 in askeleton manner. Explaining the internal structure by focusing on jointswith reference to the figure, as illustrated, the robot 1 has six jointsto be driven by eleven electric motors at each pair of right and leftleg links 2 and arm links 5.

Specifically, the robot 1 is provided at its waist (hips) a pair ofelectric motors 10R, 10L (the right-hand side is indicated by R and theleft-hand side by L; hereinafter the same) for driving joints to rotateeach leg link 2 about a vertical axis (a Z-axis or a vertical orgravitational axis), a pair of electric motors 12R, 12L for drivingjoints to rotate each leg link 2 in the pitching (advancing) direction(about a Y-axis), a pair of electric motors 14R, 14L for driving jointsto rotate each leg link 2 in the rolling (left and right) direction(about an X-axis), a pair of electric motors 16R, 16L at the knees fordriving knee joints to rotate the lower portion of each leg link 2 inthe pitching direction (about a Y-axis), a pair of electric motors 18R,18L at the ankles for driving ankle (foot) joints to rotate the distalend of each leg link 2 in the pitching axis (about a Y-axis), and a pairof electric motors 20R, 20L for driving the ankle joints to rotate thesame in the rolling axis (about an X-axis).

As mentioned above, in FIG. 3, the joints are indicated by rotating axesof the electric motors (or transmission components, such as a pulley,that are connected to the electric motors to transmit the powerthereof). A feet 22R, 22L are disposed at each distal ends of the leglink 2.

Thus, the electric motors 10R(L), 12R(L), 14R(L) are located at the hipjoints (waist joint) of the leg links 2 such that their rotating axescross orthogonally, and the electric motors 18R(L), 20R(L) are alsolocated at the ankle joints such that their rotating axes crossorthogonally. The hip joints and knee joints are connected to each otherby thigh links 24R(L), and the knee joints and ankle joints areconnected to each other by crus links 26R(L).

The leg links 2 are connected to the body 3 via the hip joints. In FIG.3, the body 3 is expressed as a body link 28 for ease of illustration.As mentioned above, the arm links 5 are connected to the body 3.

The arm links 5 are constituted in a manner similar to the leg links 2.Specifically, the robot 1 is provided at its shoulders with a pair ofelectric motors 30R, 30L for driving joints to rotate each arm link 5 inthe pitching direction, a pair of electric motors 32R, 32L for drivingjoints to rotate each arm link 5 in the rolling direction, a pair ofelectric motors 34R, 34L for driving joints to rotate the free end ofeach arm link 5, a pair of electric motors 36R, 36L at the elbows fordriving joints to rotate a portion lower than each elbow and a pair ofelectric motors 38R, 38L at the distal ends of the arms for driving thewrist joints to rotate the respective distal ends. Hands (end effectors)40R, 40L are connected to each distal end of the wrists.

The electric motors 30R(L), 32R(L), 34R(L) are located at shoulderjoints of the arm links 5 such that their rotating axes crossorthogonally. Each of the shoulder joints and elbow joints is connectedto each other by an upper arm link 42R(L) and each of the elbow jointsand wrist joints is connected to each other by a forearm link 44R(L).

The head 4 is connected to the body 3 via a neck or head joint 46 thatrotates about the vertical axis and a head swinging mechanism 48 thatrotates the head 4 about an axis perpendicular to the vertical axis. Asshown in FIG. 3, a vision sensor 50 constituted as a CCD camera thatoutputs a signal indicative of a taken image and an audio input/outputdevice 52 made up of a receiver and a microphone are disposed in thehead 4.

With the above structure, each of the left and right leg links 2 isprovided with six joints so as to be given twelve degrees of freedom intotal. When the six joints are driven to suitable angles (jointdisplacement), a desired motion is imparted to the leg links 2 to enablethe robot 1 to walk arbitrarily in an environment of three-dimensionalspace. Similarly, each of the left and right arm links 5 is providedwith five joints so as to be given ten degrees of freedom in total. Whenthe five joints are driven to suitable angles (joint displacement), itbecomes possible to control the robot 1 to perform a desired work. Also,the head 4 is provided with the joint and the head swinging mechanism soas to be given two degrees of freedom. By driving them to suitableangles, the robot can turn the head 4 in a desired direction.

The electric motors such as 10R(L) are coupled with respective rotaryencoders (not shown) that generate signals indicative of at least oneamong an angle, angular velocity and angular acceleration of acorresponding joint via rotation of rotating axes of electric motors.

A known six-axis force sensor (hereinafter referred to as “forcesensor”) 56 is disposed at each foot 22R(L), and generates a signalindicating three directional components Fx, Fy, Fz of floor reactionforce that acts on the robot from the contact surface, among externalforces acting on the robot, and three directional components Mx, My, Mzof moment of the force.

A similar force sensor (six-axis force sensor) 58 is disposed at alocation between each wrist joint and the hand 40R(L), and generates asignal indicating an external force other than the floor reaction forcethat acts on the robot, specifically, three directional components Fx,Fy, Fz of the external force (object reaction force) acting on the hand40R(L) from the object and three directional components Mx, My, Mz ofmoment of the force.

Moreover, the body 3 has an inclination sensor 60 that generates asignal indicative of at least one of inclination (inclination angle) andits angular velocity of the body 3 with respect to the vertical axis,i.e., a state variable indicating the inclination (posture) of the body3 of the robot 1.

The outputs of the force sensor 56, etc., are sent to an ElectronicControl Unit (hereinafter referred to as “ECU”) 62 comprising amicrocomputer, which is stored in the housing section 6 on the back ofthe body 3 of the robot 1 (shown in FIG. 2; for brevity of illustration,only the right side of the robot 1 is shown).

FIG. 4 is a block diagram showing details of the ECU 62, which isconstituted as a microcomputer. The outputs from the inclination sensor60, etc., are converted by an A/D converter 70 into digital values, thatare transmitted to a RAM 74 through a bus 72. The outputs from theencoders disposed adjacent to the respective electric motors inrespective actuators are transmitted to the RAM 74 through a counter 76.

The ECU 62 includes a calculator 80 comprising a CPU. The calculator 80calculates joint angle displacement commands (actuator displacementcommands) based on a gait generated by itself such that the robot cankeep a stable posture, and outputs the commands to the RAM 74. Here, thereference numeral 84 indicates a ROM.

Moreover, the calculator 80 reads out the commands and the detectedactual values from the RAM 74, and calculates control inputs(manipulated variables) necessary for driving the respective joints andoutputs the same, through a D/A converter 86 and actuator drivers(amplifiers) 88, to the electric motors 10R(L), 12R(L) etc. of a legactuators 90 and an arm actuator 92 with displacement detectors(encoders) that drive the respective joints.

FIG. 5 is a block diagram showing the configuration and operation of theposture control system of the mobile robot (that mainly corresponds tothe above-mentioned calculator 80) according to the embodiment.

This system is a system that controls the motions of the legs and armsin an unified manner, calculates the manipulated variables and outputsthe same as the actuator displacement commands to the actuator drivers88 to operate the leg actuators 90 and the arm actuators 92, as will beexplained below. As illustrated, the system includes a desired workingpattern generator 100, a leg main controller 102, an arm main controller104 and a posture stabilization main controller 106 and othercomponents.

The desired working pattern generator 100 generates the desired workingpattern including a gait of the robot 1 that satisfies the dynamicequilibrium condition at a presumed condition. The desired workingpattern is described by time variant patterns in a plurality ofvariables. The variables comprise those describing motions and thereaction forces acting from the working environment.

The variables describing motions are a combination or set of variablesthat can solely determine a robot posture at every instant.Specifically, they comprise a desired foot position/posture, a desiredbody position/posture, and a desired hand position/posture.

The variables describing the reaction force acting from the workingenvironment comprise a desired total floor reaction force central point,a desired total floor reaction force and a desired object reactionforce, as will be explained below.

These variables are expressed by a supporting leg coordinate system. Thesupporting leg coordinate system is a coordinate system whose origin isset at a point on the sole of foot 22R(L) of the supporting leg,projected vertically down from the ankle (the intersection of rotatingaxis lines of the electric motors 18, 20R(L) for driving foot joints).More specifically, as illustrated in FIGS. 6 and 7, the coordinatesystem is fixed on the floor with which the supporting leg foot sole isin contact, in which the advancing or forward direction (orientation) ofthe supporting leg foot is defined as the direction of X-axis, the leftdirection is defined as the direction of Y-axis and the vertical upwarddirection is the direction of Z-axis.

The variables will be explained in detail in the following.

The “object reaction force” indicates, among the external forces thatthe robot 1 receives in the environment, the external force other thanthe respective foot floor reaction forces. Specifically, it indicatesthe reaction force that acts on the hand 40R(L) from the object (e.g.,door handle). A desired value thereof is called the “desired objectreaction force”.

The desired object reaction force outputted from the desired workingpattern generator 100 is expressed by force and moment acting about thedesired total floor reaction force central point (explained below). Whatis significant for the posture stabilization is the moment componentthereof.

Explaining the desired total floor reaction force and desired totalfloor reaction force central point (position), the resultant force ofthe desired floor reaction forces that the respective feet receive fromthe floor during working is called the “desired total floor reactionforce in a broad sense”. The desired total floor reaction force in abroad sense is expressed by a point of action and a force and moment offorce acting about the point of action. The desired total floor reactionforce central point (position) is a floor point at which the momentcomponents about the X-axis and the Y-axis are both zero (0), when thedesired total floor reaction force is expressed by a force and momentwhose point of action is set at that floor point.

The desired total floor reaction force in a narrow sense indicates thecomponents of force and moment of the desired total floor reaction forcein the broad sense, when its point of action is set at the desire totalfloor reaction force central point. The desired total floor reactionforce that the desired working pattern generator outputs is the desiredtotal floor reaction force in the narrow sense.

In the following, the desired total floor reaction force indicates, ifnot explained to the contrary, that in the narrow sense. If the robotwalks on a flat floor, the desired total floor reaction force centralpoint is, normally, set on the floor.

The concept of the ZMP, proposed by M. Vukobratovic and known hithertoin the field of robot locomotion control, is used in this specificationin an expanded meaning. To be more specific, in case that the resultantforce of the inertial force, the gravity and the object reaction forcegenerated by the motion of the robot 1 is expressed by force and momentof force, a floor point (at which the components of the moment about theX-axis and Y-axis are both zero) is called the ZMP, using this floorpoint as the point of action. The ZMP when the robot 1 performs thedesired motion is called a “desired ZMP” and a position thereof iscalled a “desired ZMP position”.

To say that the desired working pattern satisfies the dynamicequilibrium condition indicates that the aforesaid resultant force ofthe inertial force, the gravity and the object reaction force generatedby the desired working pattern is canceled by the desired total floorreaction force and the both become zero. In order to satisfy the dynamicequilibrium condition, accordingly, the desired total floor reactionforce central point (position) and the desired ZMP (position) should bethe same.

The desired working pattern generator 100 generates the desired workingpattern including a gait that satisfies the dynamic equilibriumcondition, if a macroscopic stabilization controller (explained below)is not operating. That is to say, in this case, the desired total floorreaction force central point (position) generated by the desired workingpattern generator 100 is equal to the desired ZMP (position).

The desired foot position/posture, the desired body position/posture andthe desired hand position/posture indicate corresponding positions andpostures expressed in the aforesaid supporting leg coordinate system.Specifically, the position and velocity of the body 3 indicate arepresentative point such as the center of gravity of the body 3 and the(displacement) velocity of that point. Moreover, the postures of thebody 3 and foot 22R(L) indicate the orientation or direction of the bodyand foot in the space of X, Y, Z.

Next, the leg main controller 102 will be explained. The leg maincontroller 102 functions, briefly speaking, to implement posturetracking control such that the robot follows the desired posture and,simultaneously to implement floor reaction force control such that therobot follows the resultant force of the desired total floor reactionforce and a compensating total floor reaction force's moment (explainedbelow), by manipulating the leg actuators (electric motors 10R(L),etc.). Since it is impossible to completely satisfy both the resultantforce (of the desired floor reaction force and compensating total floorreaction force's moment) and the desired posture at the same time, thecontrol is appropriately arranged such that the both are eventuallysatisfied over a long period of time.

More specifically, the leg main controller 102 inputs a correcteddesired body position/posture (explained below), the desired footposition/posture, the desired total floor reaction force central point(position), the desired total floor reaction force (that acts about thedesired total floor reaction force central point) and the compensatingtotal floor reaction force's moment, and corrects the desired footposition/posture such that the moment components of the actual totalfloor reaction force acting about the desired total floor reaction forcecentral point (position) is equal to the sum of the desired total floorreaction force's moment (normally zero) and the compensating total floorreaction force's moment. The desired foot position/posture corrected inthis manner is called the “corrected desired foot position/posture”.

The leg main controller 102 controls the leg actuators 90 by outputtingthe actuator displacement commands such that actual joint displacementsfollow desired leg joint displacements determined by the correcteddesired body position/posture and the corrected desired footposition/posture.

For implementing the above functions, in this embodiment, the aforesaidtechnique proposed in Japanese Laid-Open Patent Application No. Hei 10(1998)-277969 (hereinafter referred to as “composite compliancecontrol”) is applied. The element or component that implements thecomposite compliance control includes, in addition to the leg maincontroller, the force sensor 56 disposed at each foot 22R(L), actuatordrivers 88 and actuators 90.

FIG. 8 is a block diagram showing this composite compliance controller.Since the controller is described in the aforesaid Japanese Laid-OpenPatent Application No. Hei 10 (1998)-277969, the detailed explanation isomitted. In the composite compliance controller illustrated in FIG. 8,compared with that described in the Japanese Laid-Open PatentApplication No. Hei 10 (1998)-277969, a posture stability control inbody inclination feedback system (calculation of the compensating totalfloor reaction force's moment) is omitted.

This is because a similar processing is implemented in a posturestabilization main controller (explained below), and the compensatingtotal floor reaction force's moment calculated there is inputted.However, in order to suppress fluctuation in the floor reaction forcedue to a body inclination error, the body inclination feedback systemmay be added.

It should be noted that, in this embodiment, the aforesaid techniqueproposed in Japanese Patent No. 3269852 (hereinafter referred to as the“macroscopic stabilization control”) must be added to the compositecompliance control. However, since the body position or stride iscorrected, more specifically, the aforesaid corrected desired bodyposition/posture is further corrected in the macroscopic stabilizationcontrol, when controlling the motion of the arm links 5 as will beexplained below, the influence of the corrected body position or stridemust be taken into account, if the relative positional relationshipbetween the hand 40R(L) and the object is significant. When themacroscopic stabilization control is operative, the desired ZMP in thedesired working pattern is controlled to a point intentionally deviatedfrom the desired total floor reaction force central point.

Since the corrected desired body position/posture is further correcteddue to the addition of the macroscopic stabilization control, thecorrected desired body position/posture, finally obtained, is called a“finally-corrected desired body position/posture” as shown in FIG. 5. Itshould be noted that to add the macroscopic stabilization control is notindispensable in the present invention. In other words, the macroscopicstabilization control may be omitted, and in that case, it suffices ifthe corrected desired body position/posture is regarded as thefinally-corrected desired body position/posture.

In FIG. 5, the finally-corrected desired foot position/posture correctedin the leg main controller 102 is inputted to the posture stabilizationmain controller 106. Notably, if change in the center of gravity of therobot due to correction of the desired foot position/posture can beneglected, it is not necessary to input the finally-corrected desiredfoot position/posture to the posture stabilization main controller 106.

Next, the arm main controller 104 will be explained. Outlining thefunction of the arm main controller 104, it implements the posturecontrol such that the arm links follow the desired posture, andsimultaneously implements object reaction force control such that thearm links follow a corrected desired object reaction force (explainedbelow), by manipulating the arm actuators (the electric motors 30R(L),etc.) 92. Here, the term “posture” indicates a combination or set ofdisplacements of all the joints of the arm links 5). Since, however, itis impossible to completely satisfy both the desired posture andcorrected desired object reaction force at the same time, an appropriateknown technique relating to a manipulator compliance control, such asthe so-called “virtual compliance control” (JSME Mechanical Engineer'sHandbook; Engineering, C4, page 100), is used.

Explaining the configuration and the algorithm of the control systemmore concretely, the arm main control system comprises, in addition tothe arm main controller 104, the force sensor 58 provided at each hand40R(L), the actuator drivers 88 and the arm actuators 92.

The arm main controller 104 inputs a final desired body position/posture(or the corrected desired body position/posture), the desired handposition/posture and the corrected desired object reaction force, andcorrects the desired hand position/posture in response to the differencebetween the actual object reaction force detected by the force sensor 58and the corrected desired object reaction force. The desired handposition/posture, thus corrected, is called a “finally-corrected desiredhand position/posture”. And the arm main controller 104 controls the armactuators 92 such that actual joint displacements follow desired armjoint displacements determined by the finally-corrected desired bodyposition/posture (or the corrected desired body position/posture) andthe finally-corrected desired hand position/posture.

Next, the posture stabilization main controller 106 will be explained.The posture stabilization main controller 106 implements control, whiletaking the dynamic equilibrium condition into account, in such a mannerthat the dynamic balance or posture balance is satisfied. Beforeentering into the explanation of the controller, the dynamic equilibriumcondition will be explained in the following.

The most dominant factor that determines posture inclination behavior ofthe robot 1 is the balance of moments of the actual force about thedesired total floor reaction force central point (i.e., the desiredZMP).

The moments of force acting about the desired total floor reaction forcecentral point can be listed up as follows.

1) the moment of inertial force

2) the moment of gravity

3) the moment of total floor reaction force

4) the moment of object reaction force

The moment of inertial force is a moment generated by the change inangular momentum of the robot about the desired total floor reactionforce central point. The moment can be calculated using Eulerianequation, and more specifically, is a value obtained by calculating thefirst differential of angular momentum of the robot 1 about the desiredtotal floor reaction force central point and then by reversing the signof the calculated value. The moment of inertial force of the desiredworking pattern is called a “desired moment of inertial force”. Themoment of inertial force actually generated when the robot 1 works iscalled an “actual moment of inertial force”. The moment of gravity is amoment generated by the gravity (which acts on the center of gravity ofthe robot 1) about the desired total floor reaction force central point.

The resultant force of the floor reaction forces respectively acting onthe two feet 22R(L) is defined as the “total floor reaction force”. Themoment of total floor reaction force is a moment generated by the totalfloor reaction force acting about the desired total floor reaction forcecentral point.

As stated above, the reaction force that the hand 40R(L) receives fromthe object is the “object reaction force”. The moment of the objectreaction force is a moment generated by the object reaction force acingabout the desired total floor reaction force central point.

Here, assuming that the robot 1 follows faithfully a motion pattern ofthe desired working pattern with the aid of an ideal leg maincontroller, the actual moment of inertial force will be equal to thedesired moment of inertial force, and the actual moment of gravity willbe equal to the desired moment of gravity.

On the other hand, according to the law of dynamics (Eulerian equation),the sum of the actual moment of inertial force, actual moment ofgravity, actual moment of total floor reaction force and actual momentof object reaction force is always zero.

Therefore, in order that the robot 1 follows faithfully the motionpattern of the desired working pattern, the sum of the desired moment ofinertial force, desired moment of gravity, actual moment of total floorreaction force and actual moment of object reaction force must be zero.This is Condition 1.

In fact, however, the actual moment of object reaction force is notequal to the desired moment of object reaction force, producing adifference or error therebetween. This is a situation that, for example,as illustrated in FIG. 9, when the robot pushes a truck (object) 108,the absolute value of the actual rolling frictional force of the truck108 falls suddenly below an expected value.

In such a situation, the moment generated by the actual object reactionforce acting about the Y-axis of the desired total floor reaction forcecentral point, exceeds the moment generated by the desired objectreaction force acting about the Y-axis of the desired total floorreaction force central point in the positive direction, thereby failingto satisfy Condition 1 and cause the robot 1 to tilt forward. It shouldbe noted that the direction of moment is defined in such a manner thatthe moment which moves the robot 1 clockwise in the positive directionon the coordinate axis, is positive.

In order to satisfy Condition 1 even under such a situation, there areproposed two methods in the technique proposed earlier.

Method 1) to vary the actual moment of total floor reaction force so asto cancel out the aforesaid difference or error. Specifically, the legmain controller 102 is instructed to generate a negative moment of floorreaction force about the desired total floor reaction force centralpoint. The leg main controller 102 produces the actuator displacementcommands, in response to the instruction, to move the toes of the feet22R(L) downward so as to increase the actual moment of total floorreaction force in the negative direction, in other words, to control therobot to take a posture to brace leg links 2.

Method 2) to correct the desired moment of inertial force and thedesired moment of gravity by correcting the motion pattern of thedesired working pattern so as to cancel out the aforesaid difference orerror. Specifically, the desired moment of inertial force and thedesired moment of gravity are corrected through the correction of thedesired body position/posture. More specifically, the body 3 iscontrolled to move backward and forward.

Since the actual moment of total floor reaction force can be variedquickly through the leg main controller by only changing the desiredmoment of total floor reaction force, Method 1) is suitable for quickresponse. However, if the actual moment of total floor reaction force isvaried greatly, the contact-pressure distribution on the soles of feet22R(L) will be partial and the contactability with the floor willdecrease. With this, at the worst, a portion or part of the foot 22R(L)will be lifted. Therefore, the desired moment of total floor reactionforce should preferably be returned to its original value over a longperiod of time.

In order to return the actual moment of total floor reaction force to anoriginal value of the desired moment of total floor reaction force, itsuffices if the motion pattern of the desired working pattern iscorrected using Method 2) such that the position of center of gravity isdisplaced (in this case, displaced backward) to cancel out the aforesaiddifference or error by the desired moment of gravity. The position ofcenter of gravity should preferably be displaced gradually, since, ifthe position of center of gravity is displaced sharply, that wouldgenerate an excessive desired moment of inertial force in the oppositedirection. Thus, Method 2) is suitable in coping with the situation overa long period of time.

In the aforesaid prior art, Japanese Laid-Open Patent Application No.Hei 10 (1998)-230485 proposed by the applicant, both the two methods areused simultaneously. Specifically, Method 1) is mainly used in a shortperiod of time so as to cope with a rapid change, while Method 2) ismainly used over a long period of time so as to converge the actualmoment of total floor reaction force to the original value of thedesired moment of total floor reaction force.

In the posture control system according to the present invention, it isfurther added with another method that deliberately changes a constraintdirection component of components of the actual object reaction force(Method 3).

The principle of Method 3) will be explained in the following. First,for ease of understanding, it is assumed that a legged mobile robot witha single arm link is controlled to work on an object using a handprovided at the distal end of the arm link. It is also assumed that thehand receives a constraint force from the object to perform a constraintmotion.

If the object is supported in the air by the hand and is not supportedby anything else, the motion of the hand is imparted with six degrees offreedom in total, (three degrees of freedom in three-dimensionalparallel translation and three degrees of freedom in three-dimensionalrotation), and the hand does not receive any constraint force from theobject.

Contrary, if the object is supported by the hand and another element,the motion of the hand may be subject to the constraint force from theelement other than the hand, depending on the situation. Specifically,when the hand contacts the object, e.g., a fixed and highly rigidsurface of the object, the hand can not be controlled to penetrate thesurface and accordingly, a positional constraint force will naturallyarise. In other words, the hand receives the constraint force from theobject surface. Noticing this constraint force, the posture controlsystem according to the present invention is arranged to implement theposture stability control of the robot using the constraint force.

Before continuing the explanation, the coordinate system and variousspaces and directions used in the later explanation will be defined asfollows. It should be noted that variables described in the followingare expressed in the supporting leg coordinate system, unless otherwisenoted.

Further, velocity components of the hand in the X, Y and Z directions ata certain moment are indicated by Vx, Vy and Vz, respectively.Similarly, rotational velocity components about the X-, Y- and Z-axes atthe same moment are indicated by ωx, ωy and ωz, respectively. As regardsthe rotation, the direction of rotation in clockwise in the positivedirection on the coordinate axis, is defined as positive.

Velocity of the hand at this moment should be expressed by a vector (Vx,Vy, Vz, ωx, ωy, ωz) and it is called a “hand velocity vector”.

Further, a set of the hand velocity vectors (Vx, Vy, Vz, ωx, ωy, ωz)whose components comprise given real numbers Vx, Vy, Vz, ωx, ωy and ωzis called a “total motion velocity space”. The total motion velocityspace is a so-called vector space.

Also, a set of all hand velocity vectors that can not be realized at acertain moment is called a “constraint velocity area” at that moment.Here, the set of hand velocity vectors (that can not be realized) arejudged from a standpoint of geometry. More precisely, the hand velocityvectors that generate stress at that moment, without causing the hand todisplace, due to interference between the hand and the object, aredefined as the hand velocity vectors that can not be realized. Here,constraints, due to location of the arm actuators or the arm joints, arenot taken into account.

That is to say, it is premised that the hand itself can move in a givenvelocity if the object does not exist. Further, if the hand and theobject have sufficient rigidity, they are regarded as rigid bodies.Consequently, when a given hand velocity vector (Vx1, Vy1, Vz1, ωx1,ωy2, ωz1) is the element of the constraint velocity area, the productobtained by multiplying by a positive given real number k, i.e., (kVx1,kVy1, kVz1, kωx1, kωy1, kωz1) is also regarded as an element of theconstraint velocity area. For convenience, a zero vector (0, 0, 0, 0, 0,0) is also regarded as an element of the constraint velocity area.

Notably, the constraint velocity area should not be confused with avelocity area comprised of the hand velocity vector that can be realizedunder the constraint condition. Besides, the constraint velocity area isnot always limited to the so-called vector space. For instance, if thehand grasps a sufficiently rigid object placed on a floor, the hand cannot move just below (the lower side in a normal direction of the floor),but can move in the opposite direction (the upper side in the normaldirection).

In other words, the hand velocity vectors in the just-below directionare the elements of the constraint velocity area, whilst those in theopposite direction are not the elements of the constraint velocity area.Assuming that the constraint velocity area is equal to the vector space,with respect to a given element in the constraint velocity area (Vx1,Vy1, Vz1, ωx1, ωy1, ωz1), its inverse vector, i.e., (−Vx1, −Vy1, −Vz1,−ωx1, −ωy1, −ωz1) must be the element of the constraint velocity area.However, since the constraint velocity area in this case does notsatisfy this condition, the constraint velocity area is not the vectorspace.

Next, a constraint velocity space is defined as follows. Specifically, aset of vectors (Vx1, Vy1, Vz1, ωx1, ωy1, ωz1) defining the constraintvelocity area and the inverse vectors thereof (−Vx1, −Vy1, −Vz1, −ωx1,−ωy1, −ωz1) are defined as the constraint velocity space. Normally, theconstraint velocity space is a partial vector space of the total motionvelocity space.

Among vectors in the total motion velocity space, a set of all thevectors whose inner products with a given vector in the constraintvelocity area are zero (i.e., those orthogonal to the given vector), iscalled a “free velocity space”.

Here, the vectors in the constraint velocity space and free velocityspace are defined as follows. A direction vector that is the element ofthe constraint velocity space is called a “constraint direction vector”.When simply named as a “constraint direction”, it means the direction ofa given constraint direction vector. A direction vector that is theelement of the free velocity space is called a “free direction vector”.When simply named as a “free direction”, it means the direction of agiven free direction vector.

The constraint velocity spaces with respect to works on several objectswill be described in the following.

Work 1) lifting an object in the air by hand.

In this case, the constraint velocity space does not exist. It ispromised, for convenience, that the constraint velocity space is solelymade up of a zero vector. The free velocity space is equal to the totalmotion velocity space.

Work 2) grasping and pushing the truck 108 with universal caster wheelsby the hand 40R, as shown in FIG. 9 and described above.

In this case, the constraint velocity space comprises a set of handvelocity vectors (0, 0, Vz, ωx, ωy, 0) that include given real numbersVz, ωx and ωy.

Work 3) opening/closing a door 114 with a hinge 112, while grasping aknob 110 by the hand 40R, as shown in FIG. 10.

In this case, motions other than rotation about a hinge axis 112Z areconstrained. Defining the hinge axis as Z-axis, X-coordinate of the handat that moment as 0 and Y-coordinate of the same as −r (r: the radius ofrotation of the door 114, more specifically, the radius of rotation ofthe knob 110), the free velocity space is constituted by a set of handvelocity vectors (Vx, 0, 0, 0, 0, Vx/r) that have a given real numberVx.

Also, the constraint velocity space is constituted by a set of handvelocity vectors (Vx, Vy, Vz, ωx, ωy, −r*Vx) that have given realnumbers Vx, Vy, Vz, ωx and ωy.

Work 4) ascending/descending stairs 122, while sliding the hand 40Ralong a cylindrical handrail 120 under the environment illustrated inFIG. 11.

In this case, motions other than a parallel translation along thelongitudinal axis of the handrail 120 and rotation about that axis ofthe handrail 120 are constrained. Expressing the axis of the handrail120 by equations x=z+c, y=−d (c, d: constants), the free velocity spacecomprises hand velocity vectors (Vx, 0, Vx, ωx, 0, ωx) that have givenreal numbers Vx and ωx. The constraint velocity space comprises a set ofhand velocity vectors (Vx, Vy, −Vx, ωx, ωy, −ωx) that have given realnumbers Vx, Vy, ωx and ωy. For ease of understanding, the handrail 120is assumed to cross with the X-axis at an angle of 45 degrees.

Work 5) ascending/descending the stairs 122, while grasping the handrail120 by the hand 40R firmly in the environment illustrated in FIG. 11.

When the handrail 120 is grasped firmly, the motions have no freedom andall motions are constrained. Accordingly, the constraint velocity spaceis equal to the total motion velocity space.

In the works mentioned above, when a force is imparted to the objectfrom the hand in its constraint direction, if only the constraint forceto cancel out the force is generated at the object from an element otherthan the hand and if no friction is substantially generated in freedirections, the motions of the hand and the object will not change.

Taking Work 3 (FIG. 10) as an example, when the hand 40R is operated togenerate an upward force (upward force in Z-axis) on the door 114, iffriction of the hinge 112 is substantively zero, since only the force tocancel out the aforesaid upward force acts from the hinge 112 to thedoor 114, this does not influence upon the rotational motion of the door114.

The posture control system according to the present invention utilizesthis understanding and is arranged to stabilize the robot posturewithout interfering with the motion control of the object. Specifically,the system is configured such that the dynamic balance is maintained oran inclined posture of the robot is restored without giving an influenceon the motion of the object, by controlling the constraint directioncomponents of force that acts on the hand from the object.

The effects of this control will be explained in the following takingWork 3 as an example. Assume a situation that, when the robot 1 standsin front of the door 114 to open/close it, the robot 1 is about to tiltforward. One method to restore the posture of the robot 1 is to push thedoor 114 forward. With this, the robot 1 can receive the reaction forcefrom the door 114 and its posture recovers.

However, if the door 114 must be pushed for the posture stabilizationcontrol, the door 114 will be accelerated and the motion thereof isdeviated from the desired opening/closing motion. Therefore, theexcessive restoring force should not be generated. In particular, if themass of the door is quite smaller than that of the robot 1, it is almostimpossible to generate the restoring force. In addition, since themotion of the door 114 is changed from the predetermined pattern, it isnot preferable from the standpoint of implementation of the work.

Another method to restore the posture of the robot 1 is to push the door114 downward. With this, the robot 1 can receive the reaction force fromthe door 114 and can recover the posture. Further, since the door isonly pushed in the constraint direction, the motion remains unchanged asmentioned above.

Since the posture control system according to the present inventionutilizes the latter method, the fact that some constraint direction mustexist in the motion of the hand as in the foregoing Works 2, 3 and 4 isa required condition for applying the present invention. Accordingly,the present invention is not applicable to the Work 1.

It should be noted that a cylindrical coordinate system or a polarcoordinate system can be used as a coordinate system expressing theconstraint velocity space or the like. It is because any normalcoordinate systems can be transformed with each other and the spaceindicated by the constraint velocity space is same at the moment oftransformation. To set or select an appropriate coordinate systemdepending on the work may occasionally make the constraint velocityspace kept constant (universal) when the work is being implemented. Forinstance, in the Work 3, by using the cylindrical coordinate system suchthat the axis of the hinge 112 is made identical to the axis of thecylindrical coordinate system, the constraint velocity space at eachmoment can be kept constant.

Next, the case that a robot having a number “n” of arm(s) is controlledto work will be discussed. Here, definitions of each spaces ordirections are expanded as follows.

First, each hand is assigned with an identification number from 1 to n,as shown in the equation below, and called “the j-th hand” (j=1, 2, . .. , n) to be discriminated.

The aforesaid supporting leg coordinate system is to be used as acoordinate system. The respective velocity components in the X, Y and Zdirections of the j-th hand at a certain moment are indicated by Vxj,Vyj and Vzj. Similarly, the rotational velocity components about the X-,Y- and Z-axes at the same moment are indicated by ωxj, ωyj and ωzj,respectively.

A vector (Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, Vx2, Vy2, Vz2, ωx2, ωy2, ωz2, .. . , Vxn, Vyn, Vzn, ωxn, ωyn, ωzn) is called a “total hand velocityvector”. The vector is made up of a component of the hand velocityvectors each arranged from the 1st hand to the n-th hand in order. Itshould be noted that, instead of the vector expression, a matrix with nrow(s) and 6 columns whose j-th column is the velocity vector of thej-th hand may be used.

From here onward, the total hand velocity vectors are renamed as “handvelocity vectors” and a set of the hand velocity vectors whosecomponents are given real numbers is renamed as a “total motion velocityspace”. This total motion velocity space comprises a 6*n-dimensionvector space.

Further, the set of all the hand velocity vectors that can not berealized at a certain moment are renamed as a “constraint velocity area”at that moment. The hand velocity vector that can not be realized isdiscriminated from the standpoint of geometry. In other words, the handvelocity vector that can generate stress at that moment, with causingalmost no hand displacement to happen, due to interference between thehand and the object or that between the hand and another hand, isdefined as the hand velocity vector that can not be realized.

At this situation, a restriction due to location of the arm actuators orthe joints of the arm links 5 or the like is not taken into account. Inother words, it is based on the premise that the hand itself can move asdesired unless the object exists. Further, if the hand or object hassufficient rigidity, they are regarded as rigid bodies. Accordingly, ifa velocity vector is the component vector constituting the constraintvelocity area, the vector obtained by multiplying all the components ofthat velocity vector by k (k: given positive real number) is alsoregarded as the vector that constitutes the constraint velocity area.For convenience, a zero vector is also regarded as the vector thatconstitutes the constraint velocity area.

A set of given vectors constituting the constraint velocity area thereofand inverse vectors (vectors with the same magnitude, but the directionis opposite) is renamed as a “constraint velocity space”. Normally, theconstraint velocity space comprises a partial vector space of the totalmotion velocity space.

Among vectors in the total motion velocity space, a set of all thevectors whose inner products with a given vector is zero (i.e.,orthogonal to the given vector), is renamed as a “free velocity space”.

Here, the vectors constituting the constraint velocity space and thefree velocity space are defined as follows. A direction vectorconstituting the constraint velocity space is renamed as a “constraintdirection vector”. When simply saying as the constraint direction, itmeans the direction of a given constraint direction vector. A directionvector constituting free velocity space is renamed as a “free directionvector”. When simply saying as the free direction, it means thedirection of a given free direction vector.

The definitions relating to the motion degree of freedom and constraintof the object(s) and the hand are expanded as stated above. With this,when the robot having a plurality of hands performs a work by using theplural hands, by operating all the hands to move in the constraintdirection to receive the reaction force (constraint force) from theobject(s), it becomes possible to maintain the dynamic balance of therobot or restore an inclined posture thereof, without giving aninfluence on the motion of the object(s).

An example of work with the use of a plurality of hands will bedescribed in the following. As illustrated in FIG. 12, assume that theobject is a round wheel 130 that has a height h and a radius r and isrotatable about Z-axis located at the front b. The work is to grasp thewheel 130 by two hands comprising a first hand and second hand and torotate it.

It is assumed here that the coordinates of the first hand are (b, −r, h)and those of the second hand are (b, r, h) at a certain moment. The freevelocity space at that moment is constituted by a set of hand velocityvectors with a given real number ωz1, i.e., (r*ωz1, 0, 0, 0, 0, ωz1,−r*ωz1, 0, 0, 0, 0, ωz1). This means a motion to rotate the two handsabout an axis of the object by the same degrees.

The constraint velocity space at that moment comprises a set of handvelocity vectors with given real numbers Vx1, Vy1, Vz1, ωx1, ωy1, ωz1,Vx2, Vy2, Vz2, ωx2 ωy2, i.e., (Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, Vx2, Vy2,Vz2, ωx2 ωy2, −r*Vx1−ωz1+r*Vx2). For instance, the hand velocity vector(Vx1, 0, 0, 0, 0, 0, Vx1, 0, 0, 0, 0, 0) is a velocity vectorconstituting the constraint velocity space. This indicates that,assuming that friction of the axis of the wheel 130 is zero, if the bothhands are operated to push in the X-direction by the same force, therotational motion of the wheel 130 will not be affected.

Another example will be explained with reference to FIG. 13. Asillustrated in the figure, the work is to open/close a door 144 with ahinge 142 by the second hand, while grasping a fixed object (e.g., acolumn) 140 by the first hand.

Assuming that the direction of the axis 142Z of the hinge 142 is inparallel with the Z-axis, it is defined that X-coordinate of the hingeaxis 142Z as b, Y-coordinate of the hinge axis as r and the coordinatesof the second hand at that moment as (b, 0, h). Defining the coordinatesof the first hand as (b, −c, h), the free velocity space comprises a setof the hand velocity vectors (0, 0, 0, 0, 0, 0, r*ωz2, 0, 0, 0, 0, ωz2)with a given real number ωz2.

Since the constraint velocity space is constituted by a set of vectorsthat cross orthogonally thereto, it comprise a set of the hand velocityvectors (Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, Vx2, Vy2, Vz2, ωx2 ωy2, −r*Vx2)having real numbers Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, Vx2, Xy2, Vz2, ωx2ωy2.

For instance, a hand velocity vector (Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, 0,0, 0, 0, 0, 0) is a velocity vector that constitutes the constraintvelocity space. This indicates that the rotational motion of the door144 is not affected when the first hand is operated to push the fixedobject 140 in any manner.

As described above, when a plurality of hands are operated to act forcesto the object in the constraint directions, if only the constraint forceto cancel out the force is generated at the object from the element(other than the hand) that supports the object and if friction or thelike is not generated in the free direction, the motions of the handsand the object do not change.

Accordingly, by expanding the definitions as stated above, in the workusing the plural hands, in accordance with the same principle as thework using one hand, it becomes possible to stabilize robot posturewithout interfering in the motion control of the object. To be morespecific, in the work that uses a plurality of hands, by controlling theconstraint direction components of force acting on the hands from theobject, it becomes possible to maintain the dynamic balance of the robotor to restore an inclined posture thereof, without giving an influenceon the motion of the object.

Generally, when the number of hands is plural, e.g., two as illustratedin this embodiment, since the dimension of the constraint velocity spacebecomes greater than the case of one hand, it becomes possible to obtainhigher effects or advantages compared to the case of one hand, bychoosing a partial vector space that is effective for stabilizing robotposture and by generating the constraint force in a direction of vectorsconstituting the space.

When it comes to the foregoing example of the work of opening/closingthe door, in order to restore the robot posture without interfering inthe opening/closing control of the door, if the robot has a single arm,it will be the most effective manner to generate a force in theupward/downward direction. When the robot has two arms, it becomespossible to maintain the dynamic balance of the robot or restore aninclined posture thereof more effectively, by operating one hand (thatdoes not grasp the door) to push a fixed object.

Making the above principle as a premise, the explanation of the posturestabilization main controller 106, illustrated in FIG. 5, will becontinued.

The posture stabilization main controller 106 uses all of the followingthree methods mentioned above, and is configured as shown in FIG. 14.

-   -   1) to correct the actual total floor reaction force's moment;    -   2) to correct the desired inertial force's moment and desired        gravity moment; and    -   3) to correct the constraint direction component from among        components of the actual object reaction force;

Specifically, the posture stabilization main controller 106 comprise aposture stabilization compensating moment calculator 200, an objectreaction force equilibrium controller 202, an object reaction forcemoment error separator 204, a corrected desired object reaction forcecalculator 206 and various distributors (precisely, an object reactionforce equilibrium control distributor 208 and a compensating momentdistributor 210).

In all of the respective constituent components of the posturestabilization main controller, calculations are repeated in everycontrol cycle (e.g., in every 100 msec.). When the ECU 62 can notimplement calculations simultaneously from its performance, thecalculation may be successively implemented from the upstream of arrowsin FIG. 14 in order.

Variables and constituent components to be processed or calculated inthe posture stabilization main controller 106 will be explained(defined) in the following.

As described above, the posture control system according to the presentinvention is a system that maintains the dynamic balance of the robot 1or restores an inclined posture thereof, without giving an influence onthe motion of the object, by controlling the constraint directioncomponent of the object reaction force. However, all the constraintdirection components of the object reaction force do not necessarilyhave effects to maintain the dynamic balance of the robot 1 or restorethe inclined posture thereof.

There is also a case that the desired object reaction force of a certainconstraint direction should not be changed in the light of the purposeof work or the nature of the object. In other words, a component to beused to maintain the dynamic balance of the robot 1 or restore ainclined posture thereof should be selected from among the constraintdirection components of the object reaction force, taking into accountthe degree of effect to restore the posture, the purpose of work and thenature of the object, appropriately.

Therefore, the space and direction are newly defined. Explaining themwith reference to FIG. 24, first, in the total motion velocity space, avector space defined by all constraint directions to be used to maintainthe dynamic balance of the robot 1 or restore an inclined posturethereof is called a “operation space”. The operation space is a partialvector space of the constraint velocity space. A given direction vectorin the operation space is called an “object reaction force operationdirection”, or simply an “operation direction”. As illustrated in thefigure, the operation space corresponds to the white-ground part that isthe constraint velocity space from which a work space is removed.

Among direction vectors in the total motion velocity space, a vectorwhose inner product with a given object reaction force operationdirection is zero (i.e., orthogonal to that direction), is called an“object reaction force non-operation direction”, or simply“non-operation direction” and a space defined by all of thenon-operation directions is called a “non-operation space”.

Specifically, the non-operation space is a partial vector space of thetotal motion velocity space, and the free velocity space is a partialvector space of the non-operation space. A given vector in the operationspace and that in the non-operation space cross orthogonally with eachother.

The operation direction can be considered as one kind of controlparameters, like a compliance constant, etc. of the compliance control.This value is not constant, but has a characteristic that changes as awork proceeds. Although it is significant how to determine this value,since it is not essential of the posture control system according to thepresent invention, for ease of explanation, it is assumed in thisembodiment that the operation direction should be determined in advancebased on the purpose or pattern of work and the nature or property ofthe object. It should also be noted that, nevertheless, the operationdirection may be determined during working.

Taking the above as a premise, the aforementioned components of theposture stabilization main controller 106 will be explained in detail.

The posture stabilization compensating moment calculator 200 is a deviceto calculate a posture stabilization compensating moment (moment usedfor correcting the actual total floor reaction force's moment) that isused to converge a posture inclination error of the robot 1, morespecifically, a body inclination error which is the difference betweenthe actual body inclination and a desired body inclination, to zero, andis the configuration to achieve the foregoing Method 1. In response atleast to the error between the body inclination angle detected by theinclination sensor 60 and the desired body inclination angle generatedby the desired working pattern generator 100, this device calculates theposture stabilization compensating moment to decrease the error.

The posture stabilization compensating moment calculator 200 isillustrated in the simplest manner in FIG. 15. In the configurationillustrated in the figure, although both inputs and outputs areexpressed in terms of scalar quantities in one dimension, in fact, theyhave X-direction component and Y-direction component and the processingshown in the figure is implemented for each of the components. SinceZ-direction component does not contribute to the posture stability, itis omitted here.

Explaining in the following, first, the body inclination error that isthe difference between the actual body inclination and the desired bodyinclination is calculated. Next, a derivative value of the bodyinclination error is calculated in a posture stabilization control law200 a, and finally, the posture stabilization compensating moment iscalculated in accordance with Eq. 1.

$\begin{matrix}{{{Posture}\mspace{11mu}{stabilization}\mspace{14mu}{compensating}\mspace{11mu}{moment}} = {{{- {kp}}*\text{Body~~inclination~~error}} - {{kd}*\text{Derivative~~value~~of~~the}\mspace{14mu}{body}\mspace{14mu}{inclination}\mspace{14mu}{error}}}} & {{Eq}.\mspace{14mu} 1}\end{matrix}$

Here, kp and kd are control gains. H infinity control or the like mayalso be used.

Continuing the explanation of FIG. 14, the object reaction forceequilibrium controller 202 is a device that, when experienced with adetectable but unexpected object reaction force, by correcting thedesired body position/posture taking the object reaction force intoaccount, implements the control to correct a desired inertial force'smoment and a desired gravity moment generated in the desired workingpattern generator 100 such that dynamic balance is maintained, i.e. is adevice that has the configuration to achieve the aforesaid Method 2).

The object reaction force moment error separator 204 is a device thatseparates the object reaction force into the constraint directioncomponent to be used for posture stabilization and components other thanthat. The corrected desired object reaction force calculator 206 is adevice that calculates the desired object reaction force that isobtained by adding the object reaction force to a desired constraintforce necessary for the posture stabilization.

The configuration of the two devices 204 and 206 and added by thevarious distributors (the object reaction force equilibrium controldistributor 208 and the compensating moment distributor 210) is theconfiguration for achieving the aforesaid Method 3). The details of thedevices will be explained later.

Explaining the object reaction force moment error separator 204 first,it is assumed that the actual hand force detected by the force sensor 58at the hand 40R(L) is a force acting on a reference point of the handand the force's moment that are expressed by a coordinate system set inthe hand locally. Here, the reference point of the hand is a point atthe hand set to express the location of the hand 40R(L). Also, theoriginal point of the aforesaid coordinate system set at the handlocally is set to the reference point of the hand.

The hand position/posture indicates a position of the original point anda direction or orientation of the coordinate system when the aforesaidcoordinate system set in the hand locally is viewed from the supportingleg coordinate system. More specifically, the position of the originalpoint is expressed by a vector and the direction or orientation of thecoordinate system is expressed by a matrix having three rows and threecolumns. Instead, a homogeneous matrix that expresses the position ofthe original point and the direction or orientation of the coordinatesystem collectively, commonly used in the field of robotics, may beused.

Further, since the hand 40R(L) can be considered to be almost at thefinal corrected desired hand position/posture by being driven by the armmain controller 104 through the actuator displacement commands, theactual hand position/posture can be considered to be almost in thefinally-corrected desired hand position/posture, the aforesaidcoordinate system set at the hand locally may be regarded as acoordinate system standardized on of the finally-corrected desired handposition/posture.

FIG. 16 is a block diagram showing the configuration of the objectreaction force moment error separator 204. Explaining this withreference to the figure, the object reaction force moment errorseparator 204 has an actual object reaction force moment error separator204 a that inputs the final corrected desired hand position/postureviewed from the supporting leg coordinate system and converts the actualhand force into a value in the supporting leg coordinate system. Theactual hand force converted into the value in the supporting legcoordinate system is also expressed by the force and the force's momentacting on the reference point of the hand. Here, what obtained byconverting the point of action of the actual hand force (converted intothe supporting leg coordinate system) into the total floor reactionforce central point is the actual object reaction force. They indicatethe same thing, although the expressions are made different.

When n hands are provided, the actual hand force converted into thesupporting leg coordinate system is expressed by a vector whosecomponents comprises the actual hand force of each hand arranged inorder. In other words, the actual hand force is expressed by a vector inthe total motion velocity space.

Specifically, it is expressed as; (F1x, F1y, F1z, M1x, M1y, M1z, F2x,F2y, F2z, M2x, M2y, M2z, . . . , Mnz). Here, Fjx, Fjy, Fjz indicate X,Y, Z-components of the force that acts on the reference point of thej-th hand, and Mjx, Mjy, Mjz indicate X, Y, Z-components of the momentof force that acts on the reference point of the j-th hand.

Next, the separator 204 a separates the actual hand force (convertedinto the value in the supporting leg coordinate system) into theoperation direction component and the non-operation direction componentin accordance with the direction of operation selected in advance. Itshould be noted here that the respective components are vectors in thetotal motion velocity space as mentioned above, and are not vectors inthe actual three-dimensional space.

Specifically, the separator 204 a separates according to the calculationmentioned in the following. First, assuming that the operation space ism-dimensional, orthogonal basic vectors in the operation space aredefined as A1, A2, . . . , Am, whilst orthogonal basic vectors in thenon-operation space are defined as B1, B2, . . . , Be. Here, the sum ofm and e is six times of the number of the hand.

Next, a vector of the actual hand force converted into the value of thesupporting leg coordinate system is defined as F. The operationdirection component of the actual hand force is defined as Fa and thenon-operation direction component of the actual hand force is defined asFb. These are vectors in the total motion velocity space.

The operation direction component of the actual hand force Fa and thenon-operation direction component of the actual hand force Fb can beobtained from Eq. 2.Fa=(A1+A2+ . . . +Am)*FFb=(B1+B2+ . . . +Bm)*F  Eq. 2

Next, the separator 204 a calculates the sum of forces that allcomponents of the operation direction component of the actual hand forceFa act about the desired total floor reaction force central point, basedon the finally-corrected desired hand position/posture and the operationdirection component of the actual hand force Fa. They are called a “sumof operation direction components of actual object reaction force aboutdesired total floor reaction force central point”. This force isexpressed by the force and moment of force in the three-dimensionalspace. The supporting leg coordinate system is used as the coordinatesystem thereof. Further, moment components in the operation directioncomponent of actual object reaction force about the desired total floorreaction force central point are called a “sum of operation directioncomponents of actual object reaction force moment about desired totalfloor reaction force central point”.

Next, the separator 204 a obtains the sum of forces that all componentsof the non-operation direction component of the actual hand force Fb actabout the desired total floor reaction force central point, based on thefinally-corrected desired hand position/posture and the non-operationdirection component of the actual hand force Fb. This is called a “sumof non-operation direction components of actual object reaction forceabout desired total floor reaction force central point”. This force isalso expressed by the force and moment of force in the three-dimensionalspace. The supporting leg coordinate system is used as the coordinatesystem thereof. Further, moment components in the non-operationdirection component of actual object reaction force about the desiredtotal floor reaction force central point are called a “sum ofnon-operation direction components of actual object reaction forcemoment about desired total floor reaction force central point”.

The object reaction force moment error separator 204 has a desired handposition action point converter 204 b. The converter 204 b alsoimplements the above-mentioned processing (calculations) for the desiredobject reaction force. The desired object reaction force is alsoexpressed by a vector constituting the total motion velocity spacesimilar to the actual hand force. Specifically, since the desired objectreaction force is expressed as force whose point of action is set at thedesired total floor reaction force central point, the converter 204 bconverts the point of action into a force and moment of force whosepoint of action is set at the desired hand position (reference pointposition of the desired hand). This is called a “desired object reactionforce whose point of action is desired hand position”.

Further, the object reaction force moment error separator 204 has adesired object reaction force moment component separator 204 c. Theseparator 204 c separates the desired object reaction force into theoperation direction component and the non-operation direction componentby performing the same processing as that mentioned in the above. Theseare called, respectively, an “operation direction component of desiredobject reaction force whose point of action is set at desired handposition” and a “non-operation direction component of desired objectreaction force whose point of action is set at desired hand position”.

Next, the separator 204 c obtains the sum of forces that all componentsof the operation direction component of the desired object reactionforce act about the desired total floor reaction force central point,based on the operation direction component of desired object reactionforce whose point of action is set at desired hand position and thedesired hand position (reference point position of the desired hand).They are called a “sum of operation direction components of desiredobject reaction force about desired total floor reaction force centralpoint”. The force is expressed by the force and moment of force in thethree-dimensional space. The supporting leg coordinate system is used asits coordinate system. Further, moment components in the operationdirection component of desired object reaction force about the desiredtotal floor reaction force central point are called a “sum of operationdirection components of desired object reaction force moment aboutdesired total floor reaction force central point”.

Next, the separator 204 c obtains the sum of forces that all componentsof the non-operation direction component of the desired object reactionforce act about the desired total floor reaction force central point,based on the non-operation direction component of desired objectreaction force whose point of action is set at desired hand position andthe desired hand position (reference point position of the desiredhand). They are called a “sum of non-operation direction components ofdesired object reaction force about desired total floor reaction forcecentral point”. The force is also expressed by the force and moment offorce in the three-dimensional space. The supporting leg coordinatesystem is also used as its coordinate system. Further, moment componentsin the non-operation direction components of desired object reactionforce about the desired total floor reaction force central point arecalled a “sum of non-operation direction components of desired objectreaction force moment about desired total floor reaction force centralpoint”.

Subsequently, at a summing point 204 d, a sum of operation directioncomponents of object reaction force error moment about desired totalfloor reaction force central point is obtained by subtracting the, thuscalculated, sum of operation direction components of desired objectreaction force moment about desired total floor reaction force centralpoint from the sum of operation direction components of actual objectreaction force moment about desired total floor reaction force centralpoint.

Further, at a summing point 204 e, a sum of non-operation directioncomponents of object reaction force error moment about desired totalfloor reaction force central point is obtained by subtracting the sum ofnon-operation direction components of desired object reaction forcemoment about desired total floor reaction force central point from thesum of non-operation direction components of actual object reactionforce moment about desired total floor reaction force central point.

It should be noted that, since components other than the momentcomponent of the object reaction force error about the desired totalfloor reaction force central point do not very contribute in maintainingthe posture of the robot 1, they are neglected in this embodiment. Also,instead of the finally-corrected desired hand position/posture, theactual hand position/posture calculated by the kinematics calculationwith the use of the actual joint angles of the leg links 2 and arm links5, may be used.

Returning to the explanation of FIG. 14, the sum of operation directioncomponents of object reaction force error moment about the desired totalfloor reaction force central point, obtained in the manner mentionedabove is utilized in calculating a compensating total floor reactionforce's moment, as will be explained below. On the other hand, the sumof non-operation direction components of object reaction force errormoment about the desired total floor reaction force central point isinputted to the object reaction force equilibrium control distributor208.

Explaining the object reaction force equilibrium control distributor 208with reference to FIG. 17, the distributor 208 comprises functions 208 aand 208 b. The object reaction force equilibrium control distributor 208inputs the sum of non-operation direction components of object reactionforce error moment about the desired total floor reaction force centralpoint to the functions 208 a and 208 b such that they output a sum ofthe object reaction force error moments that is to be equilibrated orbalanced by the object reaction force equilibrium control and a sum ofthe object reaction force error moments that is to be equilibrated orbalanced by the means other than the object reaction force equilibriumcontrol.

The object reaction force equilibrium control is a control that isimplemented in the object reaction force equilibrium controller 202 inorder to achieve the foregoing Method 2). To state briefly, as describedabove, it is the control to generate the moment of gravity by displacingthe body position such that the sum of the object reaction force errormoments is canceled out.

In the example shown in FIG. 17, although both inputs and outputs areexpressed in terms of scalar quantities in one dimension, in fact, theyhave X-direction component and Y-direction component, and the processingshown in the figure is implemented for each of the component. However,since Z-direction component is not relevant to the posture stability, itis neglected.

As shown in the figure, as the function 208 a that outputs the sum ofthe object reaction force error moments to be equilibrated by the objectreaction force equilibrium control, a function having a high and lowlimiter characteristic (saturation characteristic) is used, whereas asthe function 208 b that outputs the sum of the object reaction forceerror moments to be equilibrated by the means other than the objectreaction force equilibrium control, a function having a dead bandcharacteristic is used.

When the functions are configured to have such characteristics, in orderto maintain the dynamic balance, if the absolute value of the sum ofnon-operation direction components of object reaction force error momentabout the desired total floor reaction force central point is equal toor less than a preset value, this control system acts such that themoment is canceled out by conducting the object reaction forceequilibrium control. If the moment exceeds the preset value, the controlsystem acts such that the excess is canceled out by mainly generatingthe object reaction force in the constraint direction of the hand. Thus,by assigning the high and low limiter characteristics, it becomespossible to restrict the amount of displacement of the body position andto prevent the posture from becoming unnatural in terms of geometric.

As the two functions 208 a and 208 b that determine the input/outputrelationship, functions other than those illustrated in the figure maybe also utilized. For instance, as shown in FIG. 18, characteristics ofcurved lines 208 c and 208 d may be used. Either in the examples shownin FIG. 17 or FIG. 18, the function should be selected in accordancewith the nature or property of object or the type of work.

It should be also noted that, although the sum of outputs of the twofunctions that determine the input/output relationship should bebasically equal to the inputs, they need not be strictly the same. Inparticular, in the leg compliance control or arm compliance control, ifthe absolute value of a desired value is great, the generated forcetends to be small with respect to the desired value. For the purpose ofcorrecting this, in the area where the absolute value of the input isgreat, it is occasionally preferable to make the absolute value of thesum of outputs of functions greater than the absolute value of theinput. Further, instead of distributing the X-direction component andY-direction component independently, it is also possible to determineeach output by using a two-input function that inputs both theX-direction component and Y-direction component.

Returning to the explanation of FIG. 14, the sum of the object reactionforce error moments to be equilibrated by the means other than theobject reaction force equilibrium control, thus distributed by theobject reaction force equilibrium control distributor 208, is multipliedat a multiplication point 214 by −1 such that its polarity is reversed,and is inputted to the compensating moment distributor 210 as the objectreaction force compensating moment that is required by the means otherthan the object reaction force equilibrium control. The sum of theobject reaction force error moments to be equilibrated by the objectreaction force equilibrium control is inputted to the object reactionforce equilibrium controller 202.

Next, the object reaction force equilibrium controller 202 outlined inthe foregoing will be explained in detail.

First, explaining the function thereof, the object reaction forceequilibrium controller 202 inputs the desired body position/posture orthe like as described above, corrects the desired body position/postureand desired total floor reaction force such that they are dynamicallyequilibrated with the sum of the object reaction force error moments tobe balanced by the object reaction force equilibrium control, andoutputs the corrected desired body position/posture and a compensatingtotal floor reaction force's moment for object reaction forceequilibrium control.

In other words, the characteristic of the controller 202 is that, thegravity moment generated by displacing the desired body position/postureis dynamically equilibrated with the sum of the object reaction forceerror moments to be equilibrated or balanced by the object reactionforce equilibrium control over a long period of time such that thecorrection amount of the desired total floor reaction force returns tozero.

As discussed above, the object reaction force equilibrium controller 202is configured to achieve the Method 2) and this is a technique alreadyproposed by the applicant in Japanese Laid-Open Patent Application No.Hei 10 (1998)-230485. However, this embodiment is different from thetechnique in that only the latter part of the configuration of theobject reaction force equilibrium controller in the technique is used,and in that it is modified such that, in the element where the aforesaid“object reaction force error” is inputted, the “sum of the objectreaction force error moments to be equilibrated by the object reactionforce equilibrium control” is inputted.

FIG. 19 is a block diagram showing the configuration of the objectreaction force equilibrium controller 202 and it includes afinally-attained desired center of gravity perturbation amountcalculator 202 a, a model control law calculator 202 b and aperturbation dynamic model 202 c.

Those inputted thereto are the sum of the object reaction force errormoments to be equilibrated by the object reaction force equilibriumcontrol, desired body position/posture, finally-corrected desired handposition/posture, finally-corrected desired body position/posture andfinally-corrected desired foot position/posture. It should be notedthat, among them, when an approximate calculation is used in the objectreaction force equilibrium control, i.e., when the perturbation dynamicmodel 202 c is an approximated model, the finally-corrected desired handposition/posture, finally-corrected desired body position/posture andfinally-corrected desired foot position/posture are unnecessary.

The aforesaid posture stabilization compensating moment is not inputtedto the object reaction force equilibrium controller 202, not evenpartially. The posture stabilization compensating moment is a momentprovided to the robot from the exterior, to restore the center ofgravity displaced due to posture inclination. However, since thefunction of object reaction force equilibrium controller 202 is tocancel out the steady object reaction force error by displacing thecenter of gravity of the desired posture, it can not restore the centerof gravity displaced due to posture inclination.

The detail will be explained. The inputted sum of the object reactionforce error moments to be equilibrated by the object reaction forceequilibrium control is inputted to the finally-attained desired centerof gravity perturbation amount calculator 202 a.

An amount of center of gravity perturbation necessary for keeping theequilibrium by canceling out the sum of the object reaction force errormoments to be equilibrated by the object reaction force equilibriumcontrol over a long period of time is called a “finally-attained desiredcenter of gravity perturbation amount”. The finally-attained desiredcenter of gravity perturbation amount calculator 202 a calculates thefinally-attained center of gravity perturbation amount from theaforesaid sum and outputs it.

The finally-attained desired center of gravity perturbation amount isinputted to the model control law calculator 202 b. The model controllaw calculator 202 b will be explained. Defining that a differencebetween the finally-attained desired center of gravity perturbationamount and the desired center of gravity perturbation amount outputtedfrom the perturbation dynamic model 202 c is a center of gravitydisplacement error, the model control law calculator 202 b determinesand outputs the compensating total floor reaction force's moment forobject reaction force equilibrium control that is a moment to convergethe center of gravity displacement error to zero, in accordance with aPD control law.

The perturbation dynamic model 202 c is a model expressing therelationship between a desired total floor reaction force's momentperturbation amount for the model and a body position/postureperturbation amount, when motion (perturbation) of the desired workingpattern is assigned with a certain constraint condition.

Explaining a summing point 202 d immediately after the output of themodel control law calculator 202 b, at this summing point 202 d, thecompensating total floor reaction force's moment for object reactionforce equilibrium control is added to the sum of the object reactionforce error moments to be equilibrated by the object reaction forceequilibrium control, and the resultant sum is inputted to theperturbation dynamic model as the desired total floor reaction force'smoment perturbation amount for the model (model input amount). The bodyposition/posture perturbation amount is calculated to be a value that iscorresponding to the input by the aforesaid perturbation dynamic model.The calculated result is added to the desired body position/posture atthe summing point 202 d and the corrected desired body position/postureis outputted.

Returning to the explanation of FIG. 14, thus, the outputs of the objectreaction force equilibrium controller 202 are the corrected desired bodyposition/posture obtained by correcting the inputted desired bodyposition/posture and the compensating total floor reaction force'smoment for object reaction force equilibrium control. The outputtedcompensating total floor reaction force's moment for object reactionforce equilibrium control is, as shown in the figure, inputted to thecompensating moment distributor 210, which will be explainedsubsequently.

In the above, the reason why only the moment component among componentsof the compensating total floor reaction force for object reaction forceequilibrium control is outputted from the controller 202, is that, amongcomponents of the compensating total floor reaction force for objectreaction force equilibrium control's moment, what is significant for theposture stabilization of the robot is the moment component about X-axisor Y-axis. Although it will be better to configure the system to takecomponents other than the moment component into account, notable effectwould not be expected with this.

The compensating moment distributor 210 will be explained. As shown inFIG. 20, the compensating moment distributor 210 comprises two functionsthat input the compensating total floor reaction force's moment forobject reaction force equilibrium control, object reaction force'scompensating moment required by the means other than the object reactionforce equilibrium control and posture stabilization compensating moment,and distribute or divide them into a desired constraint force's momentand a compensating total floor reaction force's main moment.

Here, the object reaction force's compensating moment required by themeans other than the object reaction force equilibrium control is, asdescribed above, a value obtained by multiplying the sum of the objectreaction force error moments to be equilibrated by the means other thanthe object reaction force-equilibrium control by −1 at themultiplication point 214.

Further, the desired constraint force's moment is, among the aforesaidinputs, a moment to be generated about the desired total floor reactionforce central point by the object reaction force in the constraintdirection, and the compensating total floor reaction force's main momentis, among the aforesaid inputs, a moment to be generated about thedesired total floor reaction force central point by the total floorreaction force.

The distribution processing will be explained with reference to FIG. 20in the following. In the example shown in the figure, although bothinputs and outputs are expressed in scalar quantities in one dimension,in fact, there are X-direction and Y-direction components and theprocessing is conducted with respect to each component. However,Z-direction component is neglected since it is not relevant to theposture stability.

The distributor 210 has a summing point 210 a. At the summing point 210a, the compensating total floor reaction force's moment for objectreaction force equilibrium control and object reaction force'scompensating moment required by the means other than the object reactionforce equilibrium control are added and the sum is inputted to the twofunctions. Here, when the posture stabilization compensating moment isexpressed by u and the aforesaid sum is expressed by v, a value offunction g1 (u, v), one of the two functions, is obtained and the valueis determined as the compensating total floor reaction force's mainmoment. Similarly, another value of function g2 (u, v), the other one ofthe two functions, is obtained and the value is determined as thedesired constraint force's moment. The individual functions are afunction of two inputs and one output.

These functions can be expressed by the following equations, forinstance. Here, Cmax and Cmin in the equations are preset values.

When u+v>Cmax:g1(u, v)=Cmax

When u+v<Cmin:g1(u, v)=Cmin

When Cmin≦u+v≦Cmax:g1(u, v)=u+v  Eq. 3g2(u, v)=u+v−g1(u, v)  Eq. 4

In FIG. 20, the functions described by the Eqs. 3 and 4 are expressed insuch a manner that their inputs u and v are shown by orthogonal axes ona level surface and their output values are shown by height.

Basically, the two functions should be selected such that the sum ofinputs of the compensating moment distributor (sum of the compensatingtotal floor reaction force's moment for object reaction forceequilibrium control, object reaction force's compensating momentrequired by the means other than the object reaction force equilibriumcontrol and posture stabilization compensating moment) becomes equal tothe sum of outputs (sum of the desired constraint force's moment andcompensating total floor reaction force's main moment). Specifically,the two functions should be selected such that g1 (u, v)+g2 (u, v)=u+vestablishes at every instance.

Also, when terming the compensating total floor reaction force's momentfor object reaction force equilibrium control u, the object reactionforce's compensating moment required by the means other than the objectreaction force equilibrium control v and the posture stabilizationcompensating moment w, the compensating total floor reaction force'smain moment and desired constraint force's moment may be determined fromrespective values of functions g1 (u, v, w) and g2 (u, v, w) with threeinputs.

In that case, the functions should be basically selected such that g1(u, v, w)+g2 (u, v, w)=u+v+w establishes at every instance as well.However, in view of control error of the composite compliance controletc., the sum of outputs may be made greater. In particular, since theposture stabilization compensating moment is a feedback amount, even ifthe sum of output components becomes different from the posturestabilization compensating moment by a few dozen percent due toaffection by the posture stabilization compensating moment, it does notcause trouble.

Returning to the explanation of FIG. 14, the desired constraint force'smoment (one moment of moments outputted from the compensating momentdistributor 210) is sent to a summing point 215 where the moment issubtracted from the aforesaid sum of operation direction component ofobject reaction force error moment about desired total floor reactionforce central point.

On the other hand, at a summing point 216, the difference between thesum of operation direction components of object reaction force errormoment about desired total floor reaction force central point and thedesired constraint force's moment is subtracted from the compensatingtotal floor reaction force's main moment (the other moment of momentsoutputted from the compensating moment distributor 210), and thecompensating total floor reaction force's moment is outputted.

It should be noted that, if the compliance control of the arm actuators92 has high accuracy and the control can be performed such that the sumof operation direction components of object reaction force error momentabout desired total floor reaction force central point becomes almostequal to the desired constraint force's moment, the compensating totalfloor reaction force's main moment can be immediately regarded as thecompensating total floor reaction force's moment, without subtractingthe difference between the sum of operation direction components ofobject reaction force error moment about desired total floor reactionforce central point and the desired constraint force's moment at thesumming point 216.

It should be also noted that, it is possible to remove the summing point216 and to regard the compensating total floor reaction force's momentas the compensating total floor reaction force's main moment, such thatthe difference between the sum of operation direction components ofobject reaction force error moment about desired total floor reactionforce central point and the desired constraint force's moment (output ofthe summing point 215) is subtracted from the sum of the object reactionforce error moments to be equilibrated by object reaction forceequilibrium control.

The aforesaid desired constraint force's moment is also inputted to thecorrected desired object reaction force calculator 206.

The configuration of corrected desired object reaction force calculator206 is shown in FIG. 21. As illustrated in the figure, the correcteddesired object reaction force calculator 206 inputs the desired handposition/posture (or the finally-corrected desired handposition/posture), desired constraint force's moment, desired totalfloor reaction force central point position, operation direction anddesired object reaction force, and calculates the corrected desiredobject reaction force based on the inputs and outputs the calculatedresult.

The processing will be explained in the following. The calculator 206has a desired hand force correction amount determiner 206 a, a converter206 b and a summing point 206 c. The desired hand force correctionamount determiner 206 a determines, as will be described below, adesired hand force correction amount that is a correction amount of thedesired object reaction force, which is expressed so as to define thedesired hand position (or the finally-corrected desired hand position)as the point of action.

In other words, the determiner 206 a determines the desired hand forcecorrection amount such that, the difference between the sum of momentsacting about the desired total floor reaction force central point andthe desired constraint force's moment when the desired hand forcecorrection amount is imparted on the desired hand position (or thefinally-corrected desired hand position), approaches zero. Thedeterminer 206 a determines the desired hand force correction amount insuch a way that it is a force and moment of force in the operationdirection and it does not include the non-operation direction component.The desired hand force correction amount is expressed in the supportingleg coordinate system.

The simplest method of determination is described as follows. First, twovectors in the operation direction are freely selected. Each vector isobtained by linear combining orthogonal basic vectors A1, A2, . . . , Amin the operation space appropriately. These vectors are then named Q1,Q2, respectively.

Next, the desired hand force correction amount is defined as aQ1+bQ2.Here, a and b are coefficients.

Then, a moment M of a resultant force acting on the desired total floorreaction force central point when the desired hand force correctionamount aQ1+bQ2 is imparted on the desired hand position (or thefinally-corrected desired hand position) is calculated utilizing dynamiccalculation.

Expressing an X-direction component of the obtained M by Mx and aY-direction component by My, the relationship between Mx, My and a, b isexpressed as the following simultaneous linear equation. Here, k11, k12,k21 and k22 are coefficients.Mx=ak11+bk12My=ak21+bk22  Eq. 5

Next, a and b are determined such that Mx becomes equal to anX-direction component of the desired constraint force's moment and Mybecomes equal to an Y-direction component of the desired constraintforce's moment. Specifically, the simultaneous linear equation of Eq. 5is solved with respect to a and b, and by substituting the value of theX-direction component of the desired constraint force's moment for Mxand a value of the Y-direction component for My, the values a, b areobtained.

Next, the desired hand force correction amount aQ1+bQ2 is obtained byusing the vectors Q1, Q2 and the aforesaid values a, b. The obtainedcorrection amount is inputted to the converter 206 b. The converter 206b outputs the desired object reaction force correction amount whosepoint of action is set at the total floor reaction force central point,by converting expression of the desired hand force correction amount insuch a manner that its point of action is changed from the desired handposition (or the finally-corrected desired hand position) to the totalfloor reaction force central point.

Finally, by adding the desired object reaction force correction amount(whose point of action is set at the total floor reaction force centralpoint) to the desired object reaction force (whose point of action isset at the total floor reaction force central point) at the summingpoint 206 c, the corrected desired object reaction force (whose point ofaction is set at the total floor reaction force central point) isoutputted. The corrected desired object reaction force is a vector inthe total motion velocity space.

Returning to the explanation of FIG. 14, the corrected desired objectreaction force thus obtained and the aforesaid corrected desired bodyposition/posture as well as the compensating total floor reactionforce's moment are the final outputs from the posture stabilization maincontroller 106. As shown in FIG. 5, the corrected desired objectreaction force is inputted to the arm main controller 104, thecompensating total floor reaction force's moment to the leg maincontroller 102 and the corrected desired body position/posture to bothof the controllers.

The functions of the leg main controller 102 and arm main controller 104will be again explained. The leg main controller 102 corrects thedesired foot position/posture, as described above, such that the momentcomponent of the actual total floor reaction force acting about thedesired total floor reaction force central point position is equal tothe sum of the desired total floor reaction force's moment (normallyzero) and the compensating total floor reaction force's moment, andcontrols the leg joint actuators by outputting the actuator displacementcommands such that the actual joint displacements follow the desired legjoint displacements determined by the corrected desired bodyposition/posture and the corrected desired foot position/posture.

The arm main controller 104 corrects the desired hand position/posture,as described above, in response to the difference between the actualobject reaction force (actual hand force) detected by the force sensor58 at the hand 40R(L) and the corrected desired object reaction force byusing the PD control or the like such that the difference approacheszero. Further it controls the arm joint actuators by outputting theactuator displacement commands such that the actual joint displacementsfollow the desired arm joint displacements determined by the correcteddesired body position/posture (or the finally-corrected desired bodyposition/posture) and the corrected desired hand position/posture.

As explained above, the posture stabilization main controller 106 isconfigured such that it separates the object reaction force error momentacting on the desired total floor reaction force central point into theoperation direction component and non-operation direction component,cancels out a part of dynamic unbalance generated by the non-operationdirection component by the object reaction force equilibrium controller,while canceling out the rest of the dynamic unbalance by correcting theoperation direction component of desired object reaction force's momentthat must act about the desired total floor reaction force centralpoint, i.e., by correcting the motion of arm to change the operationdirection component.

In other words, in the technique proposed earlier, if the unexpectedreaction force acting from the object changes sharply, the motion tobrace legs (make legs hold) by driving the foot 22R(L) (and leg links 2)to move (controlled in the object reaction force equilibrium controller202), can be decreased or eliminated with the aid of motion of the armlinks 5. With this, since there is no possibility that the total floorreaction force's moment arises beyond a marginal limit, it becomespossible to maintain the dynamic balance of the robot 1 and prevent therobot from being tilted or tipping over.

Further, since the motion of the arm is corrected such that theoperation direction component changes, when the robot 1 is controlled tomaintain the dynamic balance or to restore a tilted posture, the motionof the object is not affected.

Finally, the stability of the aforesaid control system will beexplained.

The relationships among the desired constraint force's moment that is acorrection amount of the operation direction component of desired objectreaction force's moment that must act about the desired total floorreaction force central point, the sum of operation direction componentsof object reaction force error moment about the desired total floorreaction force central point and the sum of non-operation directioncomponents of the same are as illustrated in FIG. 14.

For ease of understanding, it is assumed to be configured such that, ofthe sum of operation direction components of object reaction force errormoment about the desired total floor reaction force central point andthe sum of non-operation direction components of the same separated bythe object reaction force moment error separator 204 in the posturestabilization main controller 106, dynamic unbalance generated by thesum of non-operation direction components is all canceled out bycorrecting the operation direction component of desired object reactionforce's moment that must act about the desired total floor reactionforce central point. In other words, it is assumed that the objectreaction force equilibrium control is not performed.

In that case, the arm control system (control system comprising the armmain controller 104, actuators 92, actuator drivers 88 and the like,shown in FIG. 5) and the control system comprising the posturestabilization main controller 106 can be simplified, as shown in FIG.22, when focusing on the object reaction force. A disturbance momentillustrated in FIG. 22 is an object reaction force generated from thereason that the object exhibits unexpected behavior due to the fact thatits characteristics are different from supposed ones, i.e., isnon-supposed object reaction force. For example, this happens whenfriction of the object is different from that supposed in advance. Anoperation direction component in the disturbance moment is called an“operation direction component of disturbance moment” and anon-operation direction component in the disturbance moment is called a“non-operation direction component of disturbance moment”.

In FIG. 22, when the non-operation direction component of disturbancemoment inputted to a summing point 220 varies, the desired constraintforce's moment is determined so as to cancel out the component and isadded at a summing point 222, in such a way that the sum of operationdirection components of desired object reaction force moment aboutdesired total floor reaction force central point is corrected, thedesired hand position/posture is corrected by the correction amount atan operation direction arm compliance controller, and the actuatordisplacement commands of the arm links 5 are then corrected, such thatthe arm links 5 of the robot 1 are driven to maintain the dynamicbalance. Even if the operation direction component of disturbance momentvaries, the desired value for the arm compliance control does not vary.

On the other hand, if it is assumed to be configured such that theposture stabilization main controller 106 cancels out the dynamicunbalance generated by the sum of non-operation direction components bycorrecting the non-operation direction component of desired objectreaction force's moment that must act about the desired total floorreaction force central point, the control system made up of the armcontrol system and posture stabilization main controller 106 can besimplified as illustrated in FIG. 23, if focusing on the object reactionforce.

In that case, the feedback loop including the posture stabilization maincontroller becomes positive and diverges or oscillates.

To be specific, the feedback loop in which the desired constraintforce's moment is calculated from the sum of non-operation directioncomponents of object reaction force error moment about desired totalfloor reaction force central point by the posture stabilization maincontroller and is added to the input (sum of non-operation directioncomponents of desired object reaction force moment about desired totalfloor reaction force central point) at a summing point 226 to determinethe sum of non-operation direction components of corrected desiredobject reaction force moment about desired total floor reaction forcecentral point that is then subtracted from the sum of non-operationdirection components of actual object reaction force moment aboutdesired total floor reaction force central point at a summing point 228to determine the sum of non-operation direction components of objectreaction force error moment about desired total floor reaction forcecentral point becomes positive and diverges or oscillates.

In other words, in the configuration shown in FIG. 23, once thenon-operation direction component of disturbance moment is generated, inorder to cancel out the generated moment, the sum of non-operationdirection components of corrected desired object reaction force momentabout desired total floor reaction force central point increases ordecreases unlimitedly, thereby rendering the system to diverge oroscillate.

In contrast, in the posture control system of a mobile robot accordingto this embodiment, as shown in FIG. 14 or the like, the object reactionforce's moment about desired total floor reaction force central point isseparated to the sum of operation direction components and the sum ofnon-operation direction components and, as shown in FIG. 22, thefeedback loop is arranged not to be positive, thereby enabling toprevent such divergence or oscillation from occurring. Specifically, itis configured such that a value fed-back on the non-operation directionside is added to the operation direction side whereby such divergence oroscillation can be prevented.

FIG. 25 is a view showing a posture control system of a mobile robotaccording to a second embodiment of the present invention.

Focusing on the points of difference from the first embodiment, in thesecond embodiment, among outputs from the object reaction forceequilibrium control distributor 208, the sum of the object reactionforce error moments to be equilibrated by object reaction forceequilibrium control is made zero (indicated by the dashed line in thefigure), whilst all outputs from the object reaction force equilibriumcontrol distributor 208 are made only the sum of the object reactionforce error moments to be equilibrated by the means other than theobject reaction force equilibrium control.

Other configuration and effects are the same as those of the firstembodiment. Since the posture control system of the mobile robotaccording to the second embodiment is thus configured, the same effectsas in the system according to the first embodiment can be achieved.

FIG. 26 is a view showing a posture control system of a mobile robotaccording to a third embodiment of the present invention.

Focusing on the points of difference from the first embodiment, in thethird embodiment, instead of the biped mobile robot 1, posture controlis implemented on a wheel mobile robot as shown in the figure.

Explaining this in the following, as illustrated, the wheel robot 300has a cylindrical body 302 and four wheels (only two wheels are shown inthe figure) 304 fixed thereto via active suspensions (not shown) thatstroke up and down (in the Z-axis direction). A mount 306 is rotatablydisposed at the top of the body 302 and a bendable arm (arm link) 310 isdisposed thereon.

To be more specific, the arm 310 comprises a first link 310 a fixed tothe mount 306, a second link 310 b connected to the first link 310 a viaa joint (not shown) and a hand 310 c connected to the free end of thesecond link 310 b via a joint (not shown). The respective jointsaccommodate actuators such as electric motors or the like.

An electronic control unit (ECU) 312 comprising a microcomputer ishoused in the body 302. An inclination sensor (not shown) is disposed inthe vicinity of the center of gravity of the body 302 and generates anoutput indicative of the inclination and its angular velocity of thebody 302 relative to the Z-axis. A load sensor (not shown) is disposedin each of the wheels 304 and detects the floor reaction forces (load)F1, F2, . . . that act on the respective four wheels from the contactsurface.

Here, defining rotational angle about the Z-axis of the mount 306 as θ1,angle relative to the Z-axis of the first link 310 a as θ2, relativeangle of the second link 310 b with respect to the first link 310 a asθ3 and relative angle of the hand 310 c with respect to the second link310 b as θ4, the desired motion of the mobile robot 300 can be expressedby motion patterns comprising θ1 to θ4 and the ZMP.

Further, in the posture control of the illustrated mobile robot 300,taking a case that the angle θ2 relative to the Z-axis of the first link310 a is to be controlled, θ2 corresponds to the desired body positionof the legged mobile robot 300 and the set of θ1, θ3, θ4 corresponds tothe desired body posture in the first embodiment. And, θ4 corresponds tothe position/posture of the hand 310 c. The operation of the electroniccontrol unit (ECU) 312 that controls the active suspensions correspondsto the operation of the composite compliance motion determiner in theconfiguration shown in FIG. 8 in the first embodiment. The electroniccontrol unit controls to distribute or divide the weight F into F1 toF4.

Accordingly, when the above-mentioned control is implemented on thewheel mobile robot 300 in the third embodiment, the same effects as theposture control of the legged mobile robot 1 described in the firstembodiment can be achieved.

As stated above, a posture control system of a mobile robot 1, 300according to the first to third embodiments is configured have at leasta body 3, a locomotive mechanism (leg links 2 or wheels 304) and atleast one arm link 5 (or arm 310) that can act a force on an object;characterized in that; when unexpected external force acts, the arm linkis driven such that, in response to a first external force that is acomponent in a predetermined direction of the unexpected external force(the sum of non-operation direction components of object reaction forceerror moment about desired total floor reaction force central point), asecond external force acts on the arm link in a direction orthogonal tothe predetermined direction (the object reaction force compensatingmoment required by the means other than the object reaction forceequilibrium control), in other words, the object reaction forcecompensating moment required by the means other than the object reactionforce equilibrium control that can be obtained by reversing a polarityof the sum of the object reaction force error moments that should beequilibrated by the means other than the object reaction forceequilibrium control which can be obtained on the basis of the sum ofnon-operation direction components of object reaction force error momentabout desired total floor reaction force central point, i.e., that canbe obtained by reversing the polarity of the sum of the object reactionforce compensating moments required by the means other than the objectreaction force equilibrium control which orthogonally intersectstherewith, i.e., that orthogonally intersects therewith, acts, wherebythe posture of the robot is stabilized without correcting a desiredvalue.

Further, the posture control system of a mobile robot 1, 300 isconfigured to have at least a body 3, a locomotive mechanism (leg links2 or wheels 304) and at least one arm link 5 (or the arm 310) that canact a force on an object; characterized in that: desired motiongenerating means (desired working pattern generator 100) for generatingdesired motion (desired object reaction force or desiredposition/posture) comprising at least a desired arm linkposition/posture of the robot (desired hand position/posture) and adesired object reaction force that is a desired value of an objectreaction force acting on the arm link from the object; actual objectreaction force detecting means (force sensor 58, load sensor, ECU 62,312 for detection and an observer for estimation) for detecting orestimating an actual object reaction force (the actual hand force) thatis an actual value of the object reaction force; posture stabilizationcontrolling means (posture stabilization main controller 106) forcorrecting the desired motion to stabilize a posture of the robot basedon at least the detected or estimated actual object reaction force; andan arm driver (actuator 90, 92) driving the arm link based on at leastthe corrected desired motion.

Further, it is configured such that the posture stabilizationcontrolling means corrects the desired motion to stabilize the postureof the robot based on at least an object reaction force's error (sum ofnon-operation direction components of object reaction force error momentabout the desired total floor reaction force central point) indicating adifference between the actual object reaction force and the desiredobject reaction force.

Further, it is configured such that the posture stabilizationcontrolling means corrects the desired motion to cancel out all or apart of tilting force generated by a first component of the objectreaction force's error (sum of non-operation direction components ofobject reaction force error moment about the desired total floorreaction force central point) by a second component (object reactionforce compensating moment required by the means other than the objectreaction force equilibrium control) obtained from the first component.

Further, the posture control system of a mobile robot 1, 300 isconfigured to have at least a body 3, a locomotive mechanism (leg links2 or wheels 304) and at least one arm link 5 (or the arm 310) that canact a force on an object; characterized in that: desired motiongenerating means (desired working pattern generator 100) for generatingdesired motion (desired object reaction force or desiredposition/posture) comprising at least a desired arm linkposition/posture of the robot (desired hand position/posture) and adesired object reaction force that is a desired value of an objectreaction force acting on the arm link from the object; posturestabilization controlling means (posture stabilization main controller106) for detecting a posture inclination error of the robot (bodyinclination angle) and for correcting the desired motion such that theposture inclination error (specifically the posture stabilizationcompensating moment calculated based on the detected value) approacheszero; and an arm driver (actuator 90, 92) driving the arm link based onat least the corrected desired motion.

It should be noted that, in the foregoing, the sum of non-operationdirection components of actual object reaction force about desired totalfloor reaction force central point may be calculated by subtracting thesum of operation direction components of actual object reaction forceabout desired total floor reaction force central point from the sum ofall components of the actual object reaction force acting about thedesired total floor reaction force central point. This is because thesum of the sum of operation direction components of actual objectreaction force about desired total floor reaction force central pointand the sum of non-operation direction components of actual objectreaction force about desired total floor reaction force central point isequal to the sum of all components of the actual object reaction forceacting about the desired total floor reaction force central point.

In contrast, the sum of operation direction components of actual objectreaction force about desired total floor reaction force central pointmay be calculated by subtracting the sum of non-operation directioncomponents of actual object reaction force about desired total floorreaction force central point from the sum of all components of theactual object reaction force acting about the desired total floorreaction force central point.

Concerning the sum of operation direction components of object reactionforce about desired total floor reaction force central point and the sumof non-operation direction components of object reaction force aboutdesired total floor reaction force central point, they may be obtainedin a similar manner.

Further, in the posture stabilization main controller 106, the desiredhand position/posture may be changed instead of correcting the desiredobject reaction force. To be more specific, since the arm maincontroller 104 normally can not control to make the object reactionforce and the hand position/posture to desired values, trade-off worksin the control of the object reaction force and the handposition/posture and as a result, the actual object reaction force maychange even when only the desired hand position/posture is varied.

Accordingly, if a ratio of a change amount of the desired handposition/posture to change of the actual object reaction force (i.e., acompliance constant of the arm main control system) at that moment isavailable beforehand, by multiplying the correction amount of thedesired object reaction force obtained in the aforesaid embodiment bythe ratio, the correction amount of the desired hand position/posturecan be obtained.

It should be also noted that, although the present invention has beendescribed with reference to the humanoid robot having two leg links andtwo arm links as a legged mobile robot, the present invention can beapplied to other legged mobile robots having any number of leg link(s)other than two or any robot that has one or more than two arm link(s).

Further, it should be also noted that, if the leg link acts on an objectother than the floor, the leg link can be considered as an arm link. Forexample, if the robot is in an insect-type with six leg links that liftsan object using the two foreleg links, the two foreleg links should beconsidered as the arm links.

Further, although the dynamic unbalance of the robot has been explainedas inclination relative to the X-axis and Y-axis directions, the presentinvention can be also applicable to spinning about the Z-axis due toswinging of a free leg or the like.

Further, although no actuator is provided for bending or twisting thebody link, if such a body actuator is added, it will be necessary toprovide a control system for a body actuator. However, since the bendingor twisting of the body link is equivalent to have additional joints atthe connecting points where the arms or legs are connected to the body,the body actuator will be regarded as a kind of arm or leg actuatorconceptually. In other words, the body actuator control system can beincluded as a part of the control system of the arm or leg.

Further, in controlling the arm link, it is alternatively possible touse any means other than the virtual compliance control, e.g., to usemeans to control the electric actuators by a current-command-typeamplifiers to control joint torques such that the object reaction forceis controlled indirectly. It will be unnecessary to dispose a forcesensor at the hand in that control, nevertheless, it will be preferableto dispose a force sensor for the object reaction force control system.

Further, it is alternatively possible to provide, instead of the forcesensor 58 provided at the hand 40R(L), an estimator that estimates theactual object reaction force from the joint torque at the arm maincontroller. A prior art disturbance observer can be used as theestimator.

Further, in some cases, the desired object reaction force may be set tozero. For example, in the Work 4 described above, if friction betweenthe handrail and the hand is almost zero, the desired object reactionforce should preferably be set to zero. Also, in the Work 2, if frictionof the universal caster wheels is unknown, it suffices if the desiredobject reaction force is set to zero. When the desired object reactionforce is constantly zero, since the object reaction force's error isequal to the actual object reaction force, instead of the objectreaction force's error, the actual object reaction force may be used inthe foregoing.

Further, even if the floor is not flat, the invention can also beapplied, by utilizing the technique, proposed by the applicant inJapanese Laid-open Patent Application No. 5 (1993)-318340, to suppose avirtual flat floor such that the desired total floor reaction forcecentral point or the desired ZMP is determined on the virtual flatfloor.

Further, it should be noted that it is possible to modify the blockdiagrams illustrated, such as, for example, changing the calculationsequence.

INDUSTRIAL FIELD IN WHICH THE INVENTION IS APPLICABLE

According to the present invention, in the posture control system of themobile robot, when unexpected external force acts, it is configured tocontrol to stabilize the posture of the robot driving the arm link(s)such that, in response to a first external force that is a component ina predetermined direction of the unexpected external force, a secondexternal force acts on the arm link(s) in a direction orthogonal to thepredetermined direction. Therefore, when the mobile robot is controlledto work that is likely to receive a reaction force from the object, evenif the posture becomes unstable or the robot receives the unexpectedreaction force acting from the object, it becomes possible to preservethe dynamic balance and to maintain a stable posture.

1. A posture control system of a mobile robot having at least a body, alocomotive mechanism and at least one arm link that can act a force onan object; comprising: a. predetermined direction component detectingmeans for detecting or estimating a first external force that is acomponent in a predetermined direction of external force acting on theat least one arm link, and b. posture stabilization controlling meansfor driving the at least one arm link in response to the first externalforce such that a second external force acts on the at least one armlink in a direction orthogonal to the predetermined direction, whereby aposture of the robot is stabilized.
 2. A posture control system of amobile robot having at least a body, a locomotive mechanism and at leastone arm link that can act a force on an object; comprising: a. desiredmotion generating means for generating desired motion comprising atleast a desired arm link position/posture of the robot and a desiredobject reaction force that is a desired value of an object reactionforce acting on the at least one arm link from the object; b. actualobject reaction force detecting means for detecting or estimating anactual object reaction force that is an actual value of the objectreaction force; c. posture stabilization controlling means forcorrecting at least one of the desired arm link position/posture and thedesired object reaction force of the desired motion to stabilize aposture of the robot based on at least the detected or estimated actualobject reaction force; and d. an arm driver driving the at least one armlink based on at least the corrected desired motion.
 3. The systemaccording to claim 2, wherein the posture stabilization controllingmeans corrects the desired motion to stabilize the posture of the robotbased on at least an object reaction force's error indicating adifference between the actual object reaction force and the desiredobject reaction force.
 4. The system according to claim 3, wherein theposture stabilization controlling means corrects the desired motion tocancel out all or a part of tilting force generated by a first componentof the object reaction force's error by a second component obtained fromthe first component.
 5. A posture control system of a mobile robothaving at least a body, a locomotive mechanism and at least one arm linkthat can act a force on an object; comprising: a. desired motiongenerating means for generating desired motion comprising at least adesired arm link position/posture of the robot and a desired objectreaction force that is a desired value of an object reaction forceacting on the at least one arm link from the object; b. posturestabilization controlling means for detecting a posture inclinationerror of the robot and for correcting at least one of the desired armlink position/posture and the desired object reaction force of thedesired motion such that the posture inclination error approaches zero;and c. an arm driver driving the at least one arm link based on at leastthe corrected desired motion.